用python将四元数换算成欧拉角

import math,cmath

with open(’/home/dji/下载/第一次运动数据Hz.txt’,‘r’) as f:
        list_read=f.readlines()

list_num=[0 for i in range(len( list_read))]
list_num[0]=list_read[0][:-1]+’,欧拉角x，欧拉角y，欧拉角z\n’

for i in range(len( list_read)-1):
        j=i+1
        a6=list_read[j].split(’,’)  #6个数
        
        w=float(a6[2])  #q0
        x=float(a6[3])   #q1
        y=float(a6[4])   #q2
        z=float(a6[5])   #q3
        
        q2sqr=yy
        t0=(-2)( q2sqr+zz )+1
        t1=2( xy+wz )
        t2=(-2)( xz-wy )
        t3=2( yz+wx )
        t4=(-2)* ( x*x+ q2sqr)
        
        if(t2>1.0):
                t2=1
        elif(t2<(-1)):
                t2=-1
                
        x=math.asin(t2)
        y=math.atan2(t3, t4)
        z=math.atan2(t1, t0)
        list_num[j]=list_read[j][:-1]+’,’+str(x)+’,’+str(y)+’,’+str(z)+’\n’
        
with open(’/home/dji/下载/第一次运动数据Hz(huan).txt’,‘w’) as f:
        for i in range(2551):
                f.write(list_num[i])