wiistate.py
Go to the documentation of this file.
00001 ################################################################################
00002 #
00003 # File:         wiistate.py
00004 # RCS:          $Header: $
00005 # Description:  Object representation of Wiimote's state
00006 # Author:       Andreas Paepcke
00007 # Created:      Thu Aug 13 11:01:34 2009 (Andreas Paepcke) paepcke@anw.willowgarage.com
00008 # Modified:     Thu Jan 13 13:50:15 2011 (Andreas Paepcke) paepcke@bhb.willowgarage.com
00009 # Language:     Python
00010 # Package:      N/A
00011 # Status:       Experimental (Do Not Distribute)
00012 #
00013 # 
00014 #
00015 ################################################################################
00016 #
00017 # Revisions:
00018 #
00019 # Thu Mar 18 10:56:09 2010 (David Lu) davidlu@wustl.edu
00020 #  Added nunchuk state variables
00021 # Fri Oct 29 08:58:21 2010 (Miguel Angel Julian Aguilar, QBO Project) miguel.angel@thecorpora.com
00022 #  Added classic controller state variables
00023 # Mon Nov 08 11:43:23 2010 (David Lu) davidlu@wustl.edu
00024 #  Added calibration for nunchuk
00025 ################################################################################
00026 
00027 from wiimoteConstants import *
00028 from wiiutils import *
00029 import numpy as np
00030 
00031 #----------------------------------------
00032 # Class WIIState
00033 #---------------
00034 
00035 class WIIState(object):
00036   """Holds the state of a WIIRemote-plus.
00037 
00038       The state is passed in and is as communicated
00039       by one message from the WII+ device. We unpack 
00040       the information and place it into individual 
00041       dictionaries for callers to grab.
00042       
00043       Public instance variables:
00044         o time             Time in fractional seconds since beginning of Epoch of when 
00045                              state was measured (Float).
00046         o ascTime          Time when state was measured (Human-readable)
00047         o rumble           True/False if wiimote vibration is on/off
00048         o angleRate        A GyroReading instance containing gyro (a.k.a. angular rate) measurement
00049         o acc              A WIIReading instance containing accelerometer measurement corrected by
00050                              the calibration information that is stored in the Wiimote
00051         o accRaw           A WIIReading instance containing accelerometer measurement uncorrected
00052         o buttons          A dictionary for which buttons are being held down. That could be
00053                              multiple buttons. Keys are:
00054                                    BTN_1, BTN_2, BTN_PLUS, BTN_MINUS, BTN_A, BTN_B,
00055                                    BTN_UP, BTN_DOWN, BTN_LEFT, BTN_RIGHT, BTN_HOME
00056                              Values are 1/0
00057         o IRSources        Dictionary with on/off values for which IR lights are
00058                            being sensed. Keys are:
00059                                    IR1, IR2, IR3, IR4
00060                            Values are 1/0
00061         o motionPlusPresent True if a gyro Motion+ is plugged into the Wiimote. Else False
00062 
00063         o nunchukPresent   True if nunchuk is plugged in. Else False
00064         o nunchukAccRaw    A WIIReading instance with acceleromoter measurement from the nunchuk (raw values)
00065         o nunchukAcc       The same, but zeroed using factory calibration
00066         o nunchukStickRaw  A tuple with the two axes of the joystick on the nunchuk, raw readings
00067         o nunchukStick     A tuple with the two axes of the joystick on the nunchuk, zeroed to be [-1, 1]
00068         o nunchukButtons   A dictionary for which nunchuk buttons are down. Keys are BTN_C and BTN_Z
00069   
00070       Public methods:
00071         o setAccelerometerCalibration   Bias setting for accelerometer. This triplet is used to
00072                                           turn raw accelerometer values into calibrated values.
00073         o setGyroCalibration            Bias setting for gyro. This triplet is used to
00074                                           turn raw gyro values into calibrated values.
00075   """
00076   
00077   _accCalibrationZero = None
00078   _gyroZeroReading = None
00079   _nunchukZeroReading = None
00080   _nunchukJoystickZero = None
00081 
00082   #----------------------------------------
00083   # __init__
00084   #----------
00085 
00086   def __init__(self, state, theTime, theRumble, buttonStatus):
00087     """Unpack the given state, normalizing if normalizers are passed in."""
00088 
00089     self.time = theTime
00090     self.ascTime = `theTime`
00091     self.rumble = theRumble
00092     self.IRSources = [None, None, None, None]
00093     self.battery = None
00094     self.acc = None
00095     self.accRaw = None
00096     self.angleRate = None
00097     self.angleRate = None
00098     self.angleRageRaw = None
00099     self.motionPlusPresent = False
00100     self.buttons   = {BTN_1: False, BTN_2: False, BTN_PLUS: False,
00101                       BTN_MINUS: False, BTN_A: False, BTN_B: False,
00102                       BTN_UP: False, BTN_DOWN: False, BTN_LEFT: False,
00103                       BTN_RIGHT: False, BTN_HOME: False}
00104     self.nunchukPresent = False
00105     self.nunchukAccRaw = None
00106     self.nunchukAcc = None
00107     self.nunchukStick = None
00108     self.nunchukStickRaw = None
00109     self.nunchukButtons = {BTN_C: False, BTN_Z: False}
00110 
00111     self.classicPresent = False
00112     self.classicStickLeft = None
00113     self.classicStickRight = None
00114     self.classicButtons = {CLASSIC_BTN_A: False, CLASSIC_BTN_B: False,
00115                            CLASSIC_BTN_L: False, CLASSIC_BTN_R: False,
00116                            CLASSIC_BTN_X: False, CLASSIC_BTN_Y: False,
00117                            CLASSIC_BTN_ZL: False, CLASSIC_BTN_ZR: False,
00118                            CLASSIC_BTN_PLUS: False, CLASSIC_BTN_MINUS: False,
00119                            CLASSIC_BTN_UP: False, CLASSIC_BTN_DOWN: False,
00120                            CLASSIC_BTN_LEFT: False, CLASSIC_BTN_RIGHT: False,
00121                            CLASSIC_BTN_HOME: False}
00122 
00123     # Handle buttons on the WII
00124     # A zero means no button is down.
00125 
00126     if buttonStatus == 0:
00127       for key in self.buttons.keys():
00128         self.buttons[key] = False
00129         continue
00130 
00131     self.buttons[BTN_1]     = (buttonStatus & BTN_1) > 0
00132     self.buttons[BTN_2]     = (buttonStatus & BTN_2) > 0
00133     self.buttons[BTN_PLUS]  = (buttonStatus & BTN_PLUS) > 0
00134     self.buttons[BTN_MINUS] = (buttonStatus & BTN_MINUS) > 0
00135     self.buttons[BTN_A]     = (buttonStatus & BTN_A) > 0
00136     self.buttons[BTN_B]     = (buttonStatus & BTN_B) > 0
00137     self.buttons[BTN_UP]    = (buttonStatus & BTN_UP) > 0
00138     self.buttons[BTN_DOWN]  = (buttonStatus & BTN_DOWN) > 0
00139     self.buttons[BTN_LEFT]  = (buttonStatus & BTN_LEFT) > 0
00140     self.buttons[BTN_RIGHT] = (buttonStatus & BTN_RIGHT) > 0
00141     self.buttons[BTN_HOME]  = (buttonStatus & BTN_HOME) > 0
00142 
00143     for msgComp in state:
00144       # msgComp has: (1,2) for Status:Button one pushed, or
00145       #              (3, [None,None,None,None]) for LEDs
00146       #              (7, {'angle_rage': (123,456,789)})
00147       msgType = msgComp[0]
00148 
00149       if msgType == WII_MSG_TYPE_ACC:
00150         # Second list member is accelerator triplet of numbers:
00151         accStatus = msgComp[1]
00152         self.accRaw = WIIReading(accStatus, self.time)
00153         
00154         # If this class knows about accelerometer calibration
00155         # data, correct the raw reading:
00156         if self._accCalibrationZero is not None and self._accCalibrationOne is not None and np.linalg.norm(self._accCalibrationOne - self._accCalibrationZero) > 1E-5:
00157             self.acc = WIIReading((self.accRaw - self._accCalibrationZero) / (self._accCalibrationOne - self._accCalibrationZero), self.time)
00158         else:
00159             self.acc = WIIReading(self.accRaw, self.time)
00160 
00161         continue
00162 
00163       elif msgType == WII_MSG_TYPE_IR:
00164         # Second list member is a list of 4 dictionaries,
00165         # one for each of the Wii IR sources:
00166 
00167         IRStatus = msgComp[1]
00168         # IRStatus is an array of four Dicts. Ex: [{'pos': (317, 445)}, None, None, None]
00169         self.IRSources[0] = IRStatus[0]
00170         self.IRSources[1] = IRStatus[1]
00171         self.IRSources[2] = IRStatus[2]
00172         self.IRSources[3] = IRStatus[3]
00173         
00174         continue
00175 
00176       elif msgType == WII_MSG_TYPE_MOTIONPLUS:
00177         # Second list member is a dictionary with the single
00178         # key 'angle_rate', which yields as its value a gyro 
00179         # readings triplet of numbers:
00180 
00181         gyroDict = msgComp[1]
00182         
00183         if gyroDict is not None:
00184             # If this class knows about a zero-movement reading for the
00185             # gyro, subtract that reading from the raw measurement:
00186             
00187             self.angleRateRaw = GyroReading(gyroDict['angle_rate'], self.time)
00188             if self._gyroZeroReading is not None:
00189                 self.angleRate = GyroReading(self.angleRateRaw - self._gyroZeroReading, self.time)
00190             else:
00191                 self.angleRate = self.angleRateRaw
00192                 
00193             self.motionPlusPresent = True
00194 
00195         continue
00196       elif msgType == WII_MSG_TYPE_NUNCHUK:
00197         nunChuk = msgComp[1];
00198         if nunChuk is not None:
00199             self.nunchukPresent = True
00200             self.nunchukAccRaw = WIIReading(nunChuk['acc'], self.time)
00201         
00202             # If this class knows about accelerometer calibration
00203             # data, correct the raw reading:
00204             if self._nunchukZeroReading is not None:
00205                 self.nunchukAcc = WIIReading((self.nunchukAccRaw - self._nunchukZeroReading) / (self._nunchukOneReading - self._nunchukZeroReading), self.time)
00206             else:
00207                 self.nunchukAcc = WIIReading(self.nunchukAccRaw, self.time)
00208 
00209 
00210             self.nunchukStickRaw = nunChuk['stick']
00211             
00212             # scale the joystick to roughly [-1, 1] 
00213             if (self._nunchukJoystickZero is None):
00214                 calibration = [127, 127]
00215             else:
00216                 calibration = self._nunchukJoystickZero
00217 
00218             [joyx, joyy] = self.nunchukStickRaw
00219             joyx = -(joyx-calibration[0])/100.
00220             joyy = (joyy-calibration[1])/100.
00221             # create a deadzone in the middle
00222             if abs(joyx) < .05:
00223                 joyx = 0
00224             if abs(joyy) < .05:
00225                 joyy = 0
00226             self.nunchukStick = [joyx,joyy]
00227 
00228             nunButtons = nunChuk['buttons']
00229             self.nunchukButtons[BTN_C]  = (nunButtons & BTN_C) > 0
00230             self.nunchukButtons[BTN_Z]  = (nunButtons & BTN_Z) > 0
00231         continue
00232       elif msgType == WII_MSG_TYPE_CLASSIC:
00233         clasSic = msgComp[1];
00234         if clasSic is not None:
00235             self.classicPresent = True
00236             self.classicStickLeft = clasSic['l_stick']
00237             self.classicStickRight = clasSic['r_stick']
00238             clasButtons = clasSic['buttons']
00239             self.classicButtons[CLASSIC_BTN_A]  = (clasButtons & CLASSIC_BTN_A) > 0
00240             self.classicButtons[CLASSIC_BTN_B]  = (clasButtons & CLASSIC_BTN_B) > 0
00241             self.classicButtons[CLASSIC_BTN_DOWN]  = (clasButtons & CLASSIC_BTN_DOWN) > 0
00242             self.classicButtons[CLASSIC_BTN_HOME]  = (clasButtons & CLASSIC_BTN_HOME) > 0
00243             self.classicButtons[CLASSIC_BTN_L]  = (clasButtons & CLASSIC_BTN_L) > 0
00244             self.classicButtons[CLASSIC_BTN_LEFT]  = (clasButtons & CLASSIC_BTN_LEFT) > 0
00245             self.classicButtons[CLASSIC_BTN_MINUS]  = (clasButtons & CLASSIC_BTN_MINUS) > 0
00246             self.classicButtons[CLASSIC_BTN_PLUS]  = (clasButtons & CLASSIC_BTN_PLUS) > 0
00247             self.classicButtons[CLASSIC_BTN_R]  = (clasButtons & CLASSIC_BTN_R) > 0
00248             self.classicButtons[CLASSIC_BTN_RIGHT]  = (clasButtons & CLASSIC_BTN_RIGHT) > 0
00249             self.classicButtons[CLASSIC_BTN_UP]  = (clasButtons & CLASSIC_BTN_UP) > 0
00250             self.classicButtons[CLASSIC_BTN_X]  = (clasButtons & CLASSIC_BTN_X) > 0
00251             self.classicButtons[CLASSIC_BTN_Y]  = (clasButtons & CLASSIC_BTN_Y) > 0
00252             self.classicButtons[CLASSIC_BTN_ZL]  = (clasButtons & CLASSIC_BTN_ZL) > 0
00253             self.classicButtons[CLASSIC_BTN_ZR]  = (clasButtons & CLASSIC_BTN_ZR) > 0
00254         continue
00255        
00256 
00257   #----------------------------------------
00258   # setAccelerometerCalibration
00259   #----------
00260   
00261   @classmethod
00262   def setAccelerometerCalibration(cls, zeroReading, oneReading):
00263       """Set the current accelerometer zeroing calibration."""
00264       cls._accCalibrationZero = WIIReading(zeroReading)
00265       cls._accCalibrationOne = WIIReading(oneReading)
00266 
00267   #----------------------------------------
00268   # getAccelerometerCalibration
00269   #----------
00270   
00271   @classmethod
00272   def getAccelerometerCalibration(cls):
00273       """Return current accelerometer zeroing offset as two lists of x/y/z: the 
00274       zero-reading, and the one-reading."""
00275       return (cls._accCalibrationZero.tuple(), cls._accCalibrationOne.tuple())
00276 
00277   #----------------------------------------
00278   # setGyroCalibration
00279   #----------
00280   
00281   @classmethod
00282   def setGyroCalibration(cls, zeroReading):
00283       """Set the x/y/z zeroing offsets for the gyro. Argument is a list"""
00284       
00285       cls._gyroZeroReading = GyroReading(zeroReading)
00286 
00287   #----------------------------------------
00288   # getGyroCalibration
00289   #----------
00290   
00291   @classmethod
00292   def getGyroCalibration(cls):
00293       """Return current gyro zeroing offset as a list of x/y/z. """
00294       return cls._gyroZeroReading.tuple()
00295 
00296   #----------------------------------------
00297   # setNunchukAccelerometerCalibration
00298   #----------
00299   
00300   @classmethod
00301   def setNunchukAccelerometerCalibration(cls, zeroReading, oneReading):
00302       """Set the current nunchuk accelerometer zeroing calibration."""
00303       cls._nunchukZeroReading = WIIReading(zeroReading)
00304       cls._nunchukOneReading = WIIReading(oneReading)
00305 
00306   #----------------------------------------
00307   # setNunchukJoystickCalibration
00308   #----------
00309   
00310   @classmethod
00311   def setNunchukJoystickCalibration(cls, readings):
00312       """Set the origin for the nunchuk joystick"""
00313       cls._nunchukJoystickZero = readings    
00314 
00315   #----------------------------------------
00316   # getNunchukAccelerometerCalibration
00317   #----------
00318   
00319   @classmethod
00320   def getNunchukAccelerometerCalibration(cls):
00321       """Return current nunchuk accelerometer zeroing offset as two lists of x/y/z: the 
00322       zero-reading, and the one-reading."""
00323       return (cls._nunchukZeroReading.tuple(), cls._nunchukOneReading.tuple())
00324 
00325 
00326   #----------------------------------------
00327   # __str___
00328   #----------
00329 
00330   def __str__(self):
00331 
00332     # Timestamp:
00333     res = 'Time: ' + self.ascTime + '\n'
00334 
00335     # Buttons that are pressed:
00336     butRes = ''
00337 
00338     if self.buttons is not None:
00339         if self.buttons[BTN_1]:
00340           butRes += ', 1'
00341         if self.buttons[BTN_2]:
00342           butRes += ', 2'
00343         if self.buttons[BTN_PLUS]:
00344           butRes += ', Plus'
00345         if self.buttons[BTN_MINUS]:
00346           butRes += ', Minus'
00347         if self.buttons[BTN_A]:
00348           butRes += ', A'
00349         if self.buttons[BTN_B]:
00350           butRes += ', B'
00351         if self.buttons[BTN_UP]:
00352           butRes += ', 4Way-up'
00353         if self.buttons[BTN_DOWN]:
00354           butRes += ', 4Way-down'
00355         if self.buttons[BTN_LEFT]:
00356           butRes += ', 4Way-left'
00357         if self.buttons[BTN_RIGHT]:
00358           butRes += ', 4Way-right'
00359         if self.buttons[BTN_HOME]:
00360           butRes += ', Home'
00361 
00362 
00363     # If none of the buttons is down, indicate that:
00364     if not butRes:
00365       res += 'Buttons: none.\n'
00366     else:
00367       res += 'Buttons: ' + butRes.lstrip(', ') + '\n'
00368       
00369 
00370     # Accelerator:
00371     if self.acc is not None:
00372         res += 'Accelerator: (' + \
00373                `self.acc[X]` + ',' + \
00374                `self.acc[Y]` + ',' + \
00375                `self.acc[Z]` + ')\n'
00376         
00377     # Gyro (angular rate):
00378 
00379     if self.angleRate is not None:
00380         res += 'Gyro (angular rate): (' + \
00381                `self.angleRate[X]` + ',' + \
00382                `self.angleRate[Y]` + ',' + \
00383                `self.angleRate[Z]` + ')\n'
00384     
00385     # Rumble status:
00386 
00387     if self.rumble:
00388       res += 'Rumble: On.\n'
00389     else:
00390       res += 'Rumble: Off.\n'
00391 
00392     # IR Sources:
00393 
00394     irRes = '' 
00395 
00396     if self.IRSources is not None:
00397         if self.IRSources[IR1] is not None:
00398           irRes += 'IR source 1'
00399     
00400         if self.IRSources[IR2] is not None:
00401           irRes += 'IR source 2'
00402     
00403         if self.IRSources[IR3] is not None:
00404           irRes += 'IR source 3'
00405     
00406         if self.IRSources[IR4] is not None:
00407           irRes += 'IR source 4'
00408     
00409         if not irRes:
00410           res += irRes.lstrip(', ') + '\n'
00411         else:
00412           res += 'No IR sources detected.\n'
00413 
00414 
00415     return res
00416 
00417   #----------------------------------------
00418   # __repr___
00419   #----------
00420 
00421   def __repr__(self):
00422       return self.__str__()
00423 
00424 
00425 #----------------------------------------
00426 # Class WIIReading
00427 #-----------------
00428 
00429 class WIIReading(object):
00430   """Instances hold one 3-D reading.
00431 
00432   Methods:
00433     [X], [Y], [Z] to obtain respective axis paramters.
00434     tuple() to obtain x/y/z as a NumPy array.
00435     +,-,/ to add or subtract readings from each other
00436         as one vector operation (pairwise for each dimension).
00437   """
00438 
00439   # Private instance vars:
00440   #     o _measurement = np.array(3, dtype=numpy.float64)
00441   #     o time
00442 
00443 
00444   def __init__(self, xyz, theTime=None):
00445     """Create a (possibly) time stamped WII Reading.
00446     
00447     Parameter xyz is an array of x,y,z coordinates of the
00448     reading. WIIReading instances can be added, subtracted, and
00449     divided into each other. The operations are pairwise over
00450     x, y, and z. A numpy array of x,y,z is available by
00451     calling tuple(). The time stamp is available via time().
00452     
00453     """ 
00454     self.time = theTime
00455     self._measurement = np.array([xyz[X], xyz[Y], xyz[Z]],dtype=np.float64)
00456 
00457   def __getitem__(self, key):
00458     if key not in (X,Y,Z):
00459         raise AttributeError("Attempt to index into a 3-D measurement array with index " + `key` + ".")
00460     return self._measurement[key]
00461 
00462     def __str__(self):
00463       return '[x=' + repr(self._measurement[X]) + \
00464              ', y=' + repr(self._measurement[Y]) + \
00465              ' z=' + repr(self._measurement[Z]) + \
00466              ']'
00467 
00468   def __repr__(self):
00469       return '[' + str(self._measurement[X]) + ', ' + str(self._measurement[Y]) + ', ' + str(self._measurement[Z]) + ']'
00470   
00471   def tuple(self):
00472     return self._measurement
00473 
00474   def __add__(self, other):
00475     """Adding two readings returns a numpy tuple with readings added pairwise."""
00476 
00477     return self._measurement + other._measurement
00478 
00479   def __sub__(self, other):
00480     """Subtracting two readings returns a numpy tuple with components subtracted pairwise."""
00481 
00482     return self._measurement - other._measurement
00483 
00484   def __div__(self, other):
00485     """Dividing two readings returns a numpy tuple with components divided pairwise."""
00486 
00487     return self._measurement / other._measurement
00488 
00489   def scale(self, scaleFactor):
00490     """Return a numpy tuple that with X, Y, Z scaled by the given factor."""
00491     
00492     return self._measurement * scaleFactor
00493 
00494 #----------------------------------------
00495 # Class GyroReading
00496 #------------------
00497 
00498 class GyroReading():
00499   """Instances hold one gyroscope reading.
00500 
00501       Methods:
00502         [PHI], [THETA], [PSI] to obtain respective axis paramters.
00503         tuple() to obtain phi/theta/psi as a NumPy array.
00504         +,-,/ to add or subtract readings from each other
00505         as one vector operation (pairwise for each dimension).
00506   """
00507   # Local instance vars:
00508   #   o _measurement = np.array(3, dtype=numpy.float64)
00509   #   o time 
00510 
00511 
00512   def __init__(self, phiThetaPsi, theTime=None):
00513     """Create a (possibly) time stamped WII Reading.
00514     
00515     Parameter phiThetaPsi is an array of phi,theta,psi coordinates of the
00516     gyro reading. GyroReading instances can be added, subtracted, and
00517     divided into each other. The operations are pairwise over
00518     phi, theta, and psi. A numpy array of phi,theta,psi is available by
00519     calling tuple(). The time stamp is available via time().
00520     """
00521 
00522     self.time = theTime
00523     self._measurement = np.array([phiThetaPsi[PHI], phiThetaPsi[THETA], phiThetaPsi[PSI]],dtype=np.float64)
00524     
00525 
00526   def __getitem__(self, key):
00527     if key not in (PHI,THETA,PSI):
00528         raise AttributeError("Attempt to index into a 3-D measurement array with index " + `key` + ".")
00529     return self._measurement[key]
00530 
00531   def __str__(self):
00532     return '[PHI (roll)=' + repr(self._measurement[PHI]) + \
00533            ', THETA (pitch)=' + repr(self._measurement[THETA]) + \
00534            ', PSI (yaw)=' + repr(self._measurement[PSI]) + \
00535            ']'
00536 
00537   def __repr__(self):
00538       '[' + str(self._measurement[PHI]) + ', ' + str(self._measurement[THETA]) + ', ' + str(self._measurement[PSI]) + ']'
00539       
00540   def tuple(self):
00541     return self._measurement
00542 
00543   def __add__(self, other):
00544     """Adding two gyro readings returns a new reading with components added pairwise."""
00545     return self._measurement + other._measurement
00546 
00547   def __sub__(self, other):
00548     """Subtracting two gyro readings returns a new reading 
00549     with components subtracted pairwise.
00550 
00551     """
00552     return self._measurement - other._measurement
00553 
00554   def __div__(self, other):
00555     """Dividing two readings returns a numpy tuple with components divided pairwise."""
00556 
00557     return self._measurement / other._measurement
00558 
00559   def scale(self, scaleFactor):
00560     """Return a numpy tuple that with X, Y, Z scaled by the given factor."""
00561     
00562     return self._measurement * scaleFactor
00563 
00564 #;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
00565 #
00566 #    Utility Functions
00567 #
00568 #;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;


wiimote
Author(s): Andreas Paepcke, Melonee Wise, Mark Horn
autogenerated on Sat Jun 8 2019 20:54:42