import serial from visual import * import pylab class Quaternion: """Quaternions for 3D rotations""" def __init__(self, q): self.q = pylab.array(q, dtype=float) self.q0 = self.q[0] self.q1 = self.q[1] self.q2 = self.q[2] self.q3 = self.q[3] self.qW = self.q[0] self.qX = self.q[1] self.qY = self.q[2] self.qZ = self.q[3] def conjugate(self): q0, q1, q2, q3 = self.q return self.__class__((q0, -q1, -q2, -q3)) def __repr__(self): return "Quaternion: " + self.q.__repr__() def __mul__(self, other): # multiplication of two quaternions. a0, a1, a2, a3 = self.q b0, b1, b2, b3 = other.q q0 = a0 * b0 - a1 * b1 - a2 * b2 - a3 * b3 q1 = a0 * b1 + a1 * b0 + a2 * b3 - a3 * b2 q2 = a0 * b2 - a1 * b3 + a2 * b0 + a3 * b1 q3 = a0 * b3 + a1 * b2 - a2 * b1 + a3 * b0 return self.__class__((q0, q1, q2, q3)) def asDCM(self): """Return the rotation matrix of the (normalized) quaternion http://www.x-io.co.uk/res/doc/madgwick_internal_report.pdf""" qw, qx, qy, qz = self.q ##http://www.rossprogrammproduct.com/translations/Matrix%20and%20Quaternion%20FAQ.htm#Q54 return pylab.mat([[1-2*(qy**2+qz**2), 2*(qx*qy-qz*qw),2*(qx*qz+qy*qw)], [2*(qx*qy+qz*qw), 1-2*(qx**2+qz**2), 2*(qy*qz-qx*qw)], [2*(qx*qz-qy*qw), 2*(qy*qz+qx*qw), 1-2*(qx**2+qy**2)]]) def asMatrix(self): return pylab.mat(self.q) def asEuler(self): q1, q2, q3, q4 = self.q yaw = atan2(2*q2*q3-2*q1*q4, 2*q1*q1+2*q2*q2-1) ##Yaw, psi, Euler[0] pitch = -pylab.arcsin(2*q2*q4+2*q1*q3) ##Pitch, theta roll = atan2(2*q3*q4-2*q1*q2, 2*q1*q1+2*q4*q4-1) ##Roll, phi return (roll, pitch, yaw) def norm(self): q0, q1, q2, q3 = self.q return sqrt(q0**2+q1**2+q2**2+q3**2) def asVector(self): return vector(self.q1, self.q2, self.q3) enSer = True def main(): if enSer: ser = serial.Serial('COM4', 115200, timeout = 1) scene=display(title="9DOF quaternion visualizer") scene.range=(2,2,2) homeLabel = label(pos=(-1.7,-1.5,0),text="Point board X axis to your monitor then press 'h'",box=0,opacity=0, xoffset = 1) label(pos=(-1.7,-1.7,0),text="Press 'e' to exit",box=0,opacity=0, xoffset = 1) label(pos=(-1.7,1.8,0),text="Quaternions:",box=0,opacity=0, xoffset = 1) lq0 = label(pos=(-1.7,1.65,0),text="q0",box=0,opacity=0, xoffset = 1) lq1 = label(pos=(-1.7,1.5,0),text="q1",box=0,opacity=0, xoffset = 1) lq2 = label(pos=(-1.7,1.35,0),text="q2",box=0,opacity=0, xoffset = 1) lq3 = label(pos=(-1.7,1.2,0),text="q3",box=0,opacity=0, xoffset = 1) label(pos=(0,1.8,0),text="Euler angles:",box=0,opacity=0, xoffset = 1) le0 = label(pos=(0,1.65,0),text="yaw (phi)",box=0,opacity=0, xoffset = 1) le1 = label(pos=(0,1.5,0),text="pitch (theta)",box=0,opacity=0, xoffset = 1) le2 = label(pos=(0,1.35,0),text="roll (psi)",box=0,opacity=0, xoffset = 1) f = frame(pos=(0,0,0.5)) ''' ax = arrow(frame=f, pos=(0,0,0), axis=(1.3,0,0), shaftwidth=0.03, color=(0,0,1)) lx = label(frame=f, pos=(1.3,0.1,0),text="X",box=0,opacity=0, xoffset = 1, color=(0,0,1)) ay = arrow(frame=f, pos=(0,0,0), axis=(0,1.1,0), shaftwidth=0.03, color=(0,1,0)) ly = label(frame=f, pos=(0.1,1.0,0),text="Y",box=0,opacity=0, xoffset = 1, color=(0,1,0)) az = arrow(frame=f, pos=(0,0,0), axis=(-0.7,-0.7,1), shaftwidth=0.01, color=(1,0,0)) lz = label(frame=f, pos=(-0.9,-0.6,1),text="Z",box=0,opacity=0, xoffset = 1, color=(1,0,0))''' eng = cone(frame=f, pos=(0,0,0), axis=(0.2,0,0), radius=0.05, color=(1,0,0)) fusel = cylinder(frame=f, pos=(0.2,0,0), axis=(1,0,0), radius=0.1, color=(0,0,1)) nose = cone(frame=f, pos=(1.2,0,0), axis=(0.2,0,0), radius=0.1, color=(0,0,1)) wingT = box(frame=f, pos=(0.9,0.025,0), axis=(1,0,0), length=0.4, width=1, height=0.05, color=(0,1,0)) wingL = box(frame=f, pos=(0.9,-0.025,0), axis=(1,0,0), length=0.4, width=1, height=0.05, color=(1,0,1)) tailL = box(frame=f, pos=(0.36,0.2,-0.025), axis=(1,0,0), length=0.3, width=0.05, height=0.3, color=(1,1,0)) tailL = box(frame=f, pos=(0.36,0.2,0.025), axis=(1,0,0), length=0.3, width=0.05, height=0.3, color=(0,1,1)) hq = None rmY = Quaternion(( sqrt(2.0)/2.0 , 0 , sqrt(2.0)/2.0 , 0 )) rmX = Quaternion(( -sqrt(2.0)/2.0 , sqrt(2.0)/2.0 , 0 , 0 )) rmZ = Quaternion(( sqrt(2.0)/2.0 , 0 , 0 , sqrt(2.0)/2.0 )) rmC = rmY*rmX while True: try: quatern = Quaternion((1.0, 0.0, 0.0, 0.0)) if enSer: l = ser.readline() else: l = '0.360423,0.439679,0.391904,0.723317,\r\n' got = l.split(',') if len(got) == 5: try: quatern = Quaternion(map(float, got[:-1])) except ValueError: pass #print 'Skipping', repr(itm) lq0.text = 'q0 = '+str(quatern.q0) lq1.text = 'q1 = '+str(quatern.q1) lq2.text = 'q2 = '+str(quatern.q2) lq3.text = 'q3 = '+str(quatern.q3) if scene.kb.keys: # event waiting to be processed? s = scene.kb.getkey() # get keyboard info if len(s) == 1: if s == 'h': hq = quatern.conjugate() if s == 'n': hq = None if s == 'e': break else: pass if hq: roll, pitch, yaw = (hq*quatern).asEuler() homeLabel.text = "Disable home position by pressing 'n'" quatern = hq*quatern else: roll, pitch, yaw = quatern.asEuler() homeLabel.text = "Point board X axis to your monitor then press 'h'" le0.text = "yaw (psi) = %.2f"%rad2deg(yaw) le1.text = "pitch (theta) = %.2f"%rad2deg(pitch) le2.text = "roll (phi) = %.2f"%rad2deg(roll) axis = Quaternion((0, 1, 0, 0)) up = Quaternion((0, 0, 0, 1)) res = rmC*quatern axis = (res*axis)*res.conjugate() up = (res*up)*res.conjugate() f.axis = axis.asVector() f.up = up.asVector() except KeyboardInterrupt: break if enSer: ser.close() print '\nExiting...' scene.visible = 0 if __name__ == '__main__': main()