martedì 15 aprile 2014

Accelerometro su Kinect

Kinect e' fornita di un accelerometro a 3 assi che secondo quando riportato da IFixIt dovrebbe essere un modello KXSD9 di Kionix

video


Per leggere i dati dell'accelorometro in Python si puo' usare il seguente script (ripreso da qui)
Attenzione: per funzionare e' necessario utilizzare PyUsb alla versione unstable 1.0 e non quella stable 0.4 che normalmente e' pacchettizzata per Linux
Prima e' quindi necessario scaricare il pacchetto unstable da GitHub
(installarea con il classico python setup.py install)
Lo script e' un demo anche per muovere il motore di tilt del Kinect
------------------------------------------------------
import usb.core
import usb.util
import sys
import time


# find our device
dev = usb.core.find(idVendor=0x045e, idProduct=0x02B0)
# was it found?
if dev is None:
    raise ValueError('Device not found')
for cfg in dev:
    sys.stdout.write("Configuration #"+str(cfg.bConfigurationValue) + '\n')
    for intf in cfg:
        sys.stdout.write('\tInterface #' + \
                         str(intf.bInterfaceNumber) + \
                         '\t, Alternate setting ' + \
                         str(intf.bAlternateSetting) + \
                         '\n')
        sys.stdout.write("\tEndpoints:\n")
        for ep in intf:
            sys.stdout.write('\t\t' + \
                             str(ep.bEndpointAddress) + \
                             '\n')


# set the active configuration. With no arguments, the first
# configuration will be the active one
dev.set_configuration()

# (bmRequestType, bmRequestType, bmRequest, wValue, wIndex, nBytes)
ret = dev.ctrl_transfer(0xC0, 0x10, 0x0, 0x0, 1)

print hex(ret[0])   # should return 0x22 but dont know why ?
# ???
ret = dev.ctrl_transfer(0x40, 0x6, 0x1, 0x0, [])

time.sleep(1.5)
# head up!
ret = dev.ctrl_transfer(0x40, 0x31, 0xfff0, 0x0, [])
time.sleep(1.5)
# bring head down
ret = dev.ctrl_transfer(0x40, 0x31, 0xffd0, 0x0, [])
time.sleep(1.5)
# up!
ret = dev.ctrl_transfer(0x40, 0x31, 0xfff0, 0x0, [])
time.sleep(1.5)
# down!
ret = dev.ctrl_transfer(0x40, 0x31, 0xffd0, 0x0, [])
time.sleep(1.5)
# up!
ret = dev.ctrl_transfer(0x40, 0x31, 0xfff0, 0x0, [])


while True:
    # Get accel data
    ret = dev.ctrl_transfer(0xC0, 0x32, 0x0, 0x0, 10)
    #print map(hex, ret)
    # bytes 0 & 1 are always zero
    x = (ret[2] << 8) | ret[3]
    x = (x + 2**15) % 2**16 - 2**15     # convert to signed 16b
    y = (ret[4] << 8) | ret[5]
    y = (y + 2**15) % 2**16 - 2**15     # convert to signed 16b
    z = (ret[6] << 8) | ret[7]
    z = (z + 2**15) % 2**16 - 2**15     # convert to signed 16b
    print x, "\t", y, "\t", z
    time.sleep(0.5)
------------------------------------------------------
(attenzione l'asse x dello script in  Python coincide con l'asse Z della codifica Microsoft mentre l'asse z dello script in Python corrisponde con quello X della codifica Microsoft)

Leggendo i dati si ha che non e' presente un sensore di azimuth per cui non e' possibile avere una orientazione nello spazio kinect. In pratica non e' possibile distinguere la rotazione sull'asse Y

Sito Microsoft


L'asse Z Microsoft mostra la rotazione destra/sinistra (mettendo di fronte al kinect appoggiato sul tavolo) e ruotando di 90° verso sinistra mostra valori positivi di 858 unita' mentre ruotando a destra di -797 unita'

L'asse X mostra la rotazione con valori di +779 unita' per posizione a 90° con i sensori che guardano lo zenith mentre di -865 unita' con i sensori che guardano nadir

Se si continua la rotazione oltre i 90° i valori tornano a decrescere verso il valore 0. Per capire se il sensore sta guardando avanti od indietro (ovvero se e' appoggiato alla sua basetta od e' appeso alla sua basetta) vengono usati i valori dell'asse Y Microsoft (y Python) che sono positivi se il Kinect guarda avanti e negativi se guarda indietro

i valori riportati sono espressi come accelerazioni in funzione dell'accelerazione di gravita'
Per calcolare l'accelerazione reale misurata la regola sembra essere
(misura/819)*9.18 m/sec*sec

Sembra quindi che i valori letti debbano essere confinati tra 0 ed 819 mentre le mie letture eccedono questo valore. Se si calcola il valore medio tra la misura maggiore e minore si ha che
asse Z (858+797)/2 = 827
asse X (779+865)/2=822
quindi considerando l'incertezza (vedi sotto) c'e' solo da calcolare un piccolo offset dello zero ed i valori coincidono con quelli di fabbrica

Mantenendo fermo il kinect ho effettuato oltre 14200 misure ed ho ottenuto una deviazione standard dei dati nei vari assi come segue
x = +/- 6.9 unita'
y = +/- 7.45 unita'
z = +/- 21.7 unita'

Per riportare i valori di inclinazione in gradi si puo' usare il seguente calcolo
-------------------------------------------------------------------------
pitch = math.atan2(y,x)*(180/3.1415926)
roll = math.atan2(y,math.sqrt((x*x)+(z*z)))*(180/3.1415926)
if (z <0):
roll = 90+(90-roll)
-------------------------------------------------------------------------
Di seguito una immagine con l'orientazione ed il valore degli angoli in relazione a diverse orientazioni