Ejemplo n.º 1
0
 def __init__(self, ID=0, SE3=None, rigidMarkers=None, meanError=0):
     self.ID = ID
     if SE3 and len(SE3) == 7:
         self.SE3 = array(SE3)
     else:
         self.SE3 = zeros((7,))
     if rigidMarkers:
         self.rigidMarkers = dict(rigidMarkers)
     else:
         self.rigidMarkers = {}
     self.meanError = meanError
     
     self.roll, self.pitch, self.yaw = map(rad2deg,
                                  rot2ZYXeuler(quaternion2rot(self.SE3[3:])))
Ejemplo n.º 2
0
    def trackInfo(self):
        '''
        Note: only for ZYX Euler angles
        '''
        # Receive the data into a buffer
#         self.__socket.recv_into(self.__buffer, self.buffersize)
#         print ' '.join(map(hex, self.__buffer))
        msg = self.__socket.recv(4096)
#         print len(msg)
#         print ' '.join(map(hex, bytearray(msg)))
        
#         Parse data 
        markerSets, rigidBodies = parseNatNet(msg)
#         markerSets, rigidBodies = parseNatNet(self.__buffer)
#         
#         for rigidBodyID in self.trackedObjects:
#             rigidBody = rigidBodies[rigidBodyID] 
#             # Check if trackable is seen
#             if not any(rigidBody.SE3):
#                 return None
#             
        # print OptiTrack data -- TODO: recode this with logging library
        if False:
            print '--------------------------------------------'
            print 'Real time info in OptiTrack system (m, deg):'
               
            # the same data displayed in the Motive/OptiTrack panel
            for rigidBodyID in self.trackedObjects:
                rigidBody = rigidBodies[rigidBodyID]
#                 rigidBody.SE3[3:] = array(list([rigidBody.SE3[6], rigidBody.SE3[3], rigidBody.SE3[5], rigidBody.SE3[4]])) # + list(rigidBody.SE3[3:6]))
                print rigidBody.SE3[3:]
                print quaternion2rot(rigidBody.SE3[3:])
                euler = rot2ZYXeuler(quaternion2rot(rigidBody.SE3[3:])) # roll, pitch, yaw
                print euler
                print rigidBodyID, '->', rigidBody.SE3[:3], map(rad2deg, euler)
          
        return rigidBodies