python 尤拉角,旋轉矩陣,四元數之間轉換
阿新 • • 發佈:2022-03-03
不論你在什麼時候開始,重要的是開始之後就不要停止。 不論你在什麼時候結束,重要的是結束之後就不要悔恨。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)