python_四元数/旋转矩阵/欧拉角之间的相互变换(简单易用版)

谁借莪１个温暖的怀抱￠ 2022-12-30 13:44 269阅读 0赞

都在代码里.

import numpy as np
    import math
    from scipy.spatial.transform import Rotation as R
    
    
    Rq=[-0.71934025092983234, 1.876085535681999e-06, 3.274841213980097e-08, 0.69465790385533299]
    
    # 四元数到旋转矩阵
    r = R.from_quat(Rq)
    Rm = r.as_matrix()
    # 0:array([ 1.00000000e+00, -2.74458557e-06,  2.55936079e-06])
    # 1:array([-2.65358979e-06, -3.49007932e-02,  9.99390782e-01])
    # 2:array([-2.65358979e-06, -9.99390782e-01, -3.49007932e-02])
    
    # 符号相反的四元数, 仍表示同一个旋转
    Rq1= [0.71934025092983234, -1.876085535681999e-06, -3.274841213980097e-08, -0.69465790385533299]
    # 四元数到旋转矩阵
    r1 = R.from_quat(Rq1)
    Rm1 = r1.as_matrix()
    # 0:array([ 1.00000000e+00, -2.74458557e-06,  2.55936079e-06])
    # 1:array([-2.65358979e-06, -3.49007932e-02,  9.99390782e-01])
    # 2:array([-2.65358979e-06, -9.99390782e-01, -3.49007932e-02])
    
    # 四元数到欧拉角
    euler0 = r.as_euler('xyz', degrees=True)
    # ([-9.20000743e+01,  1.52039496e-04, -1.52039496e-04])
    euler3 = r.as_euler('xzy', degrees=True)
    #([-9.20000743e+01, -1.52039496e-04,  1.52039496e-04])
    euler1 = r.as_euler('zxy', degrees=True)
    #([-179.99564367,  -87.99992566,  179.99579836])
    euler2 = r.as_euler('zyx', degrees=True)
    #([ 1.57253169e-04,  1.46640571e-04, -9.20000743e+01])
    
    euler4 = r.as_euler('yxz', degrees=True)
    #([179.99564367, -87.99992566, 179.99549428])
    
    euler5 = r.as_euler('yzx', degrees=True)
    #([ 1.46640571e-04,  1.57253169e-04, -9.20000743e+01])
    
    
    # 旋转矩阵到四元数
    r3 = R.from_matrix(Rm)
    qua = r3.as_quat()
    #[0.7193402509298323, -1.8760855356819988e-06, -3.2748412139801076e-08, -0.694657903855333] #与原始相反,但等价
    
    # 旋转矩阵到欧拉角
    euler_1 = r3.as_euler('zxy', degrees=True)
    #([-179.99564367,  -87.99992566,  179.99579836])
    
    # 欧拉角到旋转矩阵
    r4 = R.from_euler('zxy', [-179.99564367,  -87.99992566,  179.99579836], degrees=True)
    rm = r4.as_matrix()
    # 0:array([ 1.00000000e+00, -2.74452529e-06,  2.55936075e-06])
    # 1:array([-2.65358765e-06, -3.49007933e-02,  9.99390782e-01])
    # 2:array([-2.65352955e-06, -9.99390782e-01, -3.49007933e-02])
    
    # 欧拉角到四元数
    qua1 = r4.as_quat()
    #([-7.19340251e-01,  1.87606384e-06,  3.27274889e-08,  6.94657904e-01])
    
    
    #----测试--------------------------------------------------------------------
    theta=[-116,    0. , -105]
    r6 = R.from_euler('xyz', theta, degrees=True)
    rm = r6.as_matrix()
    # 0:array([-0.25881905, -0.42343401,  0.86816838])
    # 1:array([-0.96592583,  0.1134588 , -0.23262502])
    # 2:array([ 0.        , -0.89879405, -0.43837115])
    
    qua3 = r6.as_quat()
    #array([-0.52720286,  0.68706415, -0.39667667,  0.30438071])
    
    print(qua3)