====== 回転行列と四元数の姿勢の補間の比較 ====== ===== 目的 ===== ベクトル，回転行列，座標変換行列といった三次元幾何演算をpythonのリストをベースに[[./geo.py|geo.py]]というモジュールを自作している． 最近，回転の表現に四元数(QUATERNION)がよく用いられているが， 回転行列(MATRIX)と比較して本当にメリットがあるのかを， 特に四元数が得意とされている回転の補間を演算速度の点で比較してみることにした． このテストは以下の条件で行った． * ProBook 474s * メモリ：8 GB * CPU：Core™ i5-3230M * OS: Ubuntu 20.04 * jupyter notebook，python3 このipynb自身は，[[./matrix_vs_quaternion_2.ipynb|matrix_vs_quaternion_2.ipynb]]となっている． ===== モジュールの読み込み ===== from geo import * import time import pandas as pd ===== データの生成 ===== geo.pyには三次元ベクトル：VECTOR，三次元回転行列：MATRIX，四元数：QUATERNIONのクラスが定義されている． MATRIX,QUATERNIONのa, bはそれぞれx軸，y軸周りの回転を指定する． R1=MATRIX(a=pi/3) R2=MATRIX(b=pi/6) q1=QUATERNION(a=pi/3) q2=QUATERNION(b=pi/6) この内容はこうなる． print('R1 =', R1) print('R2 =', R2) print('q1 =', q1) print('q2 =', q2) R1 = m:[[1.0, 0.0, 0.0], [0.0, 0.5000000000000001, -0.8660254037844386], [0.0, 0.8660254037844386, 0.5000000000000001]] R2 = m:[[0.8660254037844387, 0.0, 0.49999999999999994], [0.0, 1.0, 0.0], [-0.49999999999999994, 0.0, 0.8660254037844387]] q1 = q:(0.8660254037844387, v:[0.49999999999999994, 0.0, 0.0]) q2 = q:(0.9659258262890683, v:[0.0, 0.25881904510252074, 0.0]) (R1*R2).quaternion() q:(0.8365163037378079, v:[0.4829629131445341, 0.22414386804201336, 0.1294095225512603]) q1*q2 q:(0.8365163037378079, v:[0.4829629131445341, 0.2241438680420134, 0.12940952255126034]) ===== 時間計測の関数と結果保存データ ===== def test(n,fn): i=0 start=time.time() while i< n : fn() i += 1 end = time.time() rslt=end-start return rslt data = [] def judge(test_name, m_time, q_time) : if m_time < q_time : judgment = "MATRIXの勝ち" elif m_time > q_time : judgment = "QUATERNIONの勝ち" else : judgment = "引き分け" return test_name, m_time, q_time, judgment ===== 補間のための関数 その１ ===== 補間した回転角ごとに初期姿勢からの回転変換を生成する． def interpolate_matrix(m_from, m_to, n) : m_diff=m_from.trans()*m_to th, axis = m_diff.rot_axis() # rodrigues # print(th,axis) rslt=[m_from] for i in range(1,n-1) : th_i = th*i/n m_intermed = MATRIX(axis=axis, angle=th_i) rslt.append(m_from*m_intermed) rslt.append(m_to) return rslt a=interpolate_matrix(R1,R2,100) for tmp in a : print(tmp.quaternion()) q:(0.8660254037844386, v:[0.49999999999999994, 0.0, 0.0]) q:(0.8685664923780337, v:[0.4955650769003723, 0.0027391376101021715, 0.0]) q:(0.8710783723407913, v:[0.4911134886662798, 0.005478183106975671, 0.0]) q:(0.873560959201833, v:[0.48664538499817545, 0.008217044380501116, 5.268356063861754e-09]) q:(0.8760141694753648, v:[0.48216091615190226, 0.010955629326748035, 5.268356063861754e-09]) q:(0.878437920663485, v:[0.47766023293364057, 0.01369384585107722, 0.0]) q:(0.880832131258958, v:[0.47314348669483647, 0.016431601871239196, 0.0]) q:(0.8831967207479554, v:[0.4686108293271121, 0.019168805320474883, 0.0]) q:(0.8855316096127639, v:[0.46406241325715775, 0.02190536415060079, 7.450580596923828e-09]) q:(0.8878367193344588, v:[0.4594983914416061, 0.02464118633511576, 0.0]) q:(0.8901119723955451, v:[0.45491891736188833, 0.02737617987228907, 0.0]) q:(0.8923572922825634, v:[0.4503241450190728, 0.030110252788255857, 7.450580596923828e-09]) q:(0.8945726034886643, v:[0.4457142289286865, 0.03284331314011253, 0.0]) q:(0.8967578315161462, v:[0.4410893241155184, 0.035575269019003215, 0.0]) q:(0.8989129028789612, v:[0.4364495861084065, 0.03830602855321763, 0.0]) q:(0.9010377451051865, v:[0.43179517093500774, 0.041035499911274524, 5.268356063861754e-09]) q:(0.9031322867394616, v:[0.42712623511655085, 0.0437635913050123, 0.0]) q:(0.9051964573453898, v:[0.42244293566257246, 0.046490210992675966, 0.0]) q:(0.9072301875079093, v:[0.417745430065638, 0.04921526728200174, 0.0]) q:(0.9092334088356255, v:[0.4130338762960443, 0.0519386685333009, 7.450580596923828e-09]) 　　　　　　　　　: : q:(0.9716336282114709, v:[0.11084991759724824, 0.2089028202234153, 0.0]) q:(0.9716883266316954, v:[0.10559563563200959, 0.21135599736739819, 0.0]) q:(0.9717103485834275, v:[0.10033780263876534, 0.21380206691586973, 0.0]) q:(0.971699693326101, v:[0.09507659543080955, 0.21624094661106066, 0.0]) q:(0.9716563612180366, v:[0.08981219093490717, 0.21867255443698513, 7.450580596923828e-09]) q:(0.9715803537164303, v:[0.08454476618534205, 0.22109680862220088, 0.0]) q:(0.971471673377304, v:[0.07927449831796592, 0.22351362764255686, 0.0]) q:(0.9713303238554201, v:[0.07400156456423883, 0.22592293022393642, 0.0]) q:(0.9711563099041576, v:[0.06872614224527315, 0.2283246353449894, 0.0]) q:(0.9709496373753531, v:[0.06344840876586723, 0.230718662239858, 0.0]) q:(0.9707103132191043, v:[0.058168541608537835, 0.23310493040089109, 0.0]) q:(0.970438345483535, v:[0.0528867183275577, 0.23548335958135297, 0.0]) q:(0.9701337433145255, v:[0.04760311654297964, 0.23785386979812154, 0.0]) q:(0.9697965169554049, v:[0.04231791393466445, 0.24021638133437792, 0.0]) q:(0.9694266777466063, v:[0.03703128823630815, 0.24257081474228692, 5.268356063861754e-09]) q:(0.9690242381252857, v:[0.03174341722946072, 0.24491709084566954, 5.268356063861754e-09]) q:(0.9685892116249034, v:[0.026454478737552207, 0.247255130742665, 0.0]) q:(0.9681216128747697, v:[0.021164650619910557, 0.24958485580838377, 0.0]) q:(0.9676214575995519, v:[0.01587411076577773, 0.25190618769755274, 0.0]) q:(0.967088762618746, v:[0.010583037088333424, 0.25421904834714837, 0.0]) q:(0.9659258262890683, v:[0.0, 0.2588190451025207, 0.0]) def interpolate_quaternion(q_from,q_to, n) : q_diff = q_from.conjugate()*q_to si = abs(q_diff.v) if si != 0.0 : axis = 1.0/si * q_diff.v th = atan2(si,q_diff.w) # print(th,axis) rslt=[q_from] for i in range(1,n-1) : th_i = th*i/n q_intermed=QUATERNION([cos(th_i),sin(th_i)*axis]) rslt.append(q_from*q_intermed) rslt.append(q_to) return rslt b = interpolate_quaternion(q1, q2, 100) b [q:(0.8660254037844387, v:[0.49999999999999994, 0.0, 0.0]), q:(0.8685664923780337, v:[0.49556507690037227, 0.002739137610098667, 2.168404344971009e-19]), q:(0.8710783723407913, v:[0.4911134886662798, 0.00547818310697403, 0.0]), q:(0.873560959201833, v:[0.48664538499817545, 0.00821704438050042, 4.336808689942018e-19]), q:(0.8760141694753648, v:[0.4821609161519022, 0.010955629326747329, 0.0]), q:(0.8784379206634849, v:[0.47766023293364057, 0.01369384585107674, 8.673617379884035e-19]), q:(0.8808321312589579, v:[0.4731434866948364, 0.01643160187124013, 0.0]), q:(0.8831967207479554, v:[0.46861082932711207, 0.019168805320475046, -1.734723475976807e-18]), q:(0.8855316096127639, v:[0.46406241325715775, 0.02190536415060122, 0.0]), q:(0.8878367193344587, v:[0.4594983914416061, 0.024641186335115972, 0.0]), : : q:(0.9716883266316954, v:[0.10559563563200969, 0.21135599736739816, 0.0]), q:(0.9717103485834275, v:[0.10033780263876535, 0.2138020669158698, 0.0]), q:(0.971699693326101, v:[0.09507659543080976, 0.2162409466110606, 0.0]), q:(0.9716563612180366, v:[0.08981219093490717, 0.21867255443698527, 0.0]), q:(0.9715803537164303, v:[0.08454476618534201, 0.22109680862220094, 0.0]), q:(0.9714716733773041, v:[0.07927449831796574, 0.22351362764255686, 0.0]), q:(0.97133032385542, v:[0.07400156456423912, 0.22592293022393642, 0.0]), q:(0.9711563099041576, v:[0.06872614224527368, 0.22832463534498953, 0.0]), q:(0.9709496373753532, v:[0.06344840876586683, 0.230718662239858, 0.0]), q:(0.9707103132191042, v:[0.05816854160853774, 0.23310493040089106, 1.3877787807814457e-17]), q:(0.970438345483535, v:[0.05288671832755765, 0.235483359581353, 1.3877787807814457e-17]), q:(0.9701337433145255, v:[0.047603116542979496, 0.2378538697981216, 0.0]), q:(0.969796516955405, v:[0.04231791393466472, 0.24021638133437795, 1.3877787807814457e-17]), q:(0.9694266777466064, v:[0.03703128823630819, 0.242570814742287, 0.0]), q:(0.9690242381252857, v:[0.03174341722946106, 0.24491709084566965, 0.0]), q:(0.9685892116249035, v:[0.0264544787375528, 0.247255130742665, 0.0]), q:(0.9681216128747696, v:[0.02116465061991013, 0.2495848558083839, 1.3877787807814457e-17]), q:(0.9676214575995518, v:[0.015874110765777494, 0.25190618769755274, 0.0]), q:(0.967088762618746, v:[0.010583037088333302, 0.25421904834714837, -1.3877787807814457e-17]), q:(0.9659258262890683, v:[0.0, 0.25881904510252074, 0.0])] ===== 補間のための関数 その２ ===== 補間のための増分に対応する回転変換を逐次演算する．高速化されるが誤差の累積があり得る． def interpolate_matrix2(m_from, m_to, n) : m_diff=m_from.trans()*m_to th, axis = m_diff.rot_axis() # rodrigues # print(th,axis) m_delta=MATRIX(axis=axis,angle=th/n) m_intermed=MATRIX(mat=m_from) rslt=[m_from] for i in range(1,n-1) : m_intermed=m_intermed*m_delta rslt.append(m_intermed) rslt.append(m_to) return rslt c=interpolate_matrix2(R1,R2,100) for tmp in c : print(tmp.quaternion()) q:(0.8660254037844386, v:[0.49999999999999994, 0.0, 0.0]) q:(0.8685664923780337, v:[0.4955650769003723, 0.0027391376101021715, 0.0]) q:(0.8710783723407913, v:[0.4911134886662798, 0.005478183106975671, 0.0]) q:(0.873560959201833, v:[0.4866453849981754, 0.008217044380506184, 0.0]) q:(0.8760141694753648, v:[0.48216091615190226, 0.010955629326750567, 0.0]) q:(0.8784379206634849, v:[0.4776602329336406, 0.01369384585108026, 0.0]) q:(0.8808321312589579, v:[0.4731434866948365, 0.01643160187124342, 0.0]) q:(0.8831967207479554, v:[0.4686108293271122, 0.01916880532047778, 0.0]) q:(0.8855316096127639, v:[0.46406241325715786, 0.021905364150603956, 0.0]) q:(0.8878367193344587, v:[0.4594983914416062, 0.024641186335118574, 0.0]) q:(0.890111972395545, v:[0.45491891736188844, 0.027376179872292114, 0.0]) q:(0.8923572922825632, v:[0.450324145019073, 0.030110252788259084, 0.0]) q:(0.8945726034886642, v:[0.4457142289286866, 0.03284331314011549, 0.0]) q:(0.896757831516146, v:[0.4410893241155185, 0.035575269019006726, 0.0]) q:(0.898912902878961, v:[0.43644958610840656, 0.03830602855322089, 7.450580596923828e-09]) q:(0.9010377451051864, v:[0.43179517093500785, 0.04103549991127757, 5.268356063861754e-09]) q:(0.9031322867394612, v:[0.4271262351165509, 0.04376359130501547, 0.0]) q:(0.9051964573453896, v:[0.4224429356625726, 0.04649021099267925, 7.450580596923828e-09]) q:(0.907230187507909, v:[0.4177454300656382, 0.04921526728200541, 7.450580596923828e-09]) q:(0.9092334088356253, v:[0.41303387629604454, 0.05193866853330437, 0.0]) q:(0.9112060539631117, v:[0.4083084327965086, 0.05466032316254457, 0.0]) : : q:(0.9716883266316939, v:[0.10559563563201564, 0.21135599736740054, 2.1721988524665314e-08]) q:(0.971710348583426, v:[0.10033780263877183, 0.21380206691587214, 2.2351741790771484e-08]) q:(0.9716996933260995, v:[0.09507659543081672, 0.2162409466110629, 2.1073424255447017e-08]) q:(0.9716563612180351, v:[0.08981219093491474, 0.21867255443698747, 2.1721988524665314e-08]) q:(0.9715803537164288, v:[0.0845447661853501, 0.2210968086222032, 2.1721988524665314e-08]) q:(0.9714716733773026, v:[0.07927449831797449, 0.22351362764255917, 2.1721988524665314e-08]) q:(0.9713303238554185, v:[0.07400156456424876, 0.2259229302239387, 2.1721988524665314e-08]) q:(0.9711563099041559, v:[0.06872614224528406, 0.22832463534499184, 2.1721988524665314e-08]) q:(0.9709496373753517, v:[0.06344840876587816, 0.23071866223986032, 2.2351741790771484e-08]) q:(0.9707103132191027, v:[0.05816854160855024, 0.23310493040089333, 2.29642316809631e-08]) q:(0.9704383454835334, v:[0.052886718327571615, 0.23548335958135527, 2.356080457693621e-08]) q:(0.970133743314524, v:[0.047603116542995094, 0.23785386979812392, 2.29642316809631e-08]) q:(0.9697965169554033, v:[0.04231791393468248, 0.24021638133438028, 2.356080457693621e-08]) q:(0.9694266777466046, v:[0.037031288236328765, 0.24257081474228928, 2.526614808710392e-08]) q:(0.969024238125284, v:[0.031743417229485635, 0.24491709084567187, 2.47107803102985e-08]) q:(0.9685892116249017, v:[0.026454478737582634, 0.24725513074266725, 2.414264045062609e-08]) q:(0.968121612874768, v:[0.021164650619947274, 0.2495848558083861, 2.414264045062609e-08]) q:(0.9676214575995501, v:[0.015874110765828436, 0.2519061876975549, 2.414264045062609e-08]) q:(0.9670887626187442, v:[0.010583037088410793, 0.2542190483471506, 2.47107803102985e-08]) q:(0.9659258262890683, v:[0.0, 0.2588190451025207, 0.0]) def interpolate_quaternion2(q_from,q_to, n) : q_diff = q_from.conjugate()*q_to si = abs(q_diff.v) if si != 0.0 : axis = 1.0/si * q_diff.v th = atan2(si,q_diff.w) # print(th,axis) q_delta=QUATERNION([cos(th/n),sin(th/n)*axis]) q_intermed=QUATERNION([q_from.w,q_from.v]) rslt=[q_from] for i in range(1,n-1) : th_i = th*i/n q_intermed=q_intermed*q_delta rslt.append(q_intermed) rslt.append(q_to) return rslt d=interpolate_quaternion2(q1,q2,100) d [q:(0.8660254037844387, v:[0.49999999999999994, 0.0, 0.0]), q:(0.8685664923780337, v:[0.49556507690037227, 0.002739137610098667, 2.168404344971009e-19]), q:(0.8710783723407913, v:[0.4911134886662797, 0.00547818310697403, 4.980553729855286e-19]), q:(0.8735609592018329, v:[0.4866453849981753, 0.008217044380500419, 4.3029273720518457e-19]), q:(0.8760141694753646, v:[0.48216091615190204, 0.010955629326747327, 4.336808689942018e-19]), q:(0.8784379206634847, v:[0.47766023293364035, 0.013693845851076735, 3.9302328752599536e-19]), q:(0.8808321312589575, v:[0.47314348669483625, 0.01643160187124012, 6.2341624917916505e-19]), q:(0.883196720747955, v:[0.46861082932711184, 0.019168805320475036, 8.944667923005412e-19]), q:(0.8855316096127634, v:[0.4640624132571575, 0.021905364150601212, 1.0977546996415732e-18]), q:(0.8878367193344583, v:[0.45949839144160587, 0.024641186335115965, 1.4230153513872246e-18]), q:(0.8901119723955445, v:[0.4549189173618881, 0.027376179872288946, 1.8973538018496328e-18]), q:(0.8923572922825629, v:[0.45032414501907264, 0.03011025278825601, 2.3310346708438345e-18]), q:(0.8945726034886636, v:[0.4457142289286863, 0.032843313140112164, 2.791820594150174e-18]), q:(0.8967578315161454, v:[0.44108932411551816, 0.035575269019003486, 3.2526065174565133e-18]), q:(0.8989129028789605, v:[0.43644958610840623, 0.03830602855321787, 3.848917712323541e-18]), q:(0.901037745105186, v:[0.43179517093500747, 0.041035499911274545, 4.336808689942018e-18]), q:(0.9031322867394608, v:[0.4271262351165505, 0.04376359130501225, 4.526544070126981e-18]), q:(0.905196457345389, v:[0.42244293566257224, 0.0464902109926759, 4.797594613248357e-18]), q:(0.9072301875079085, v:[0.41774543006563775, 0.049215267282001755, 5.3939058081153846e-18]), q:(0.9092334088356246, v:[0.41303387629604416, 0.0519386685333009, 5.9631119486702744e-18]), q:(0.911206053963111, v:[0.40830843279650814, 0.054660323162540936, 6.667843360785852e-18]), q:(0.9131480565531743, v:[0.40356925847683794, 0.05738013964442585, 7.37257477290143e-18]), : : q:(0.9716336282114678, v:[0.1108499175972483, 0.20890282022341475, 4.336808689942018e-17]), q:(0.9716883266316922, v:[0.10559563563200967, 0.21135599736739757, 4.466912950640278e-17]), q:(0.9717103485834244, v:[0.10033780263876536, 0.21380206691586923, 4.597017211338539e-17]), q:(0.9716996933260977, v:[0.09507659543080976, 0.21624094661106003, 4.7271214720367993e-17]), q:(0.9716563612180333, v:[0.08981219093490717, 0.2186725544369847, 4.87890977618477e-17]), q:(0.971580353716427, v:[0.08454476618534208, 0.22109680862220032, 5.0306980803327406e-17]), q:(0.9714716733773008, v:[0.07927449831796572, 0.22351362764255628, 5.204170427930421e-17]), q:(0.9713303238554167, v:[0.07400156456423924, 0.22592293022393575, 5.355958732078392e-17]), q:(0.9711563099041541, v:[0.0687261422452737, 0.2283246353449889, 5.4860629927766524e-17]), q:(0.9709496373753498, v:[0.06344840876586694, 0.23071866223985735, 5.681219383824043e-17]), q:(0.9707103132191008, v:[0.05816854160853785, 0.23310493040089042, 5.854691731421724e-17]), q:(0.9704383454835315, v:[0.052886718327557765, 0.23548335958135236, 6.028164079019405e-17]), q:(0.970133743314522, v:[0.04760311654297963, 0.23785386979812093, 6.223320470066795e-17]), q:(0.9697965169554014, v:[0.042317913934664886, 0.2402163813343772, 6.418476861114186e-17]), q:(0.9694266777466027, v:[0.03703128823630836, 0.24257081474228626, 6.591949208711867e-17]), q:(0.969024238125282, v:[0.031743417229461314, 0.2449170908456689, 6.765421556309548e-17]), q:(0.9685892116248997, v:[0.02645447873755296, 0.24725513074266423, 6.938893903907228e-17]), q:(0.968121612874766, v:[0.021164650619910446, 0.24958485580838305, 7.090682208055199e-17]), q:(0.967621457599548, v:[0.015874110765777768, 0.25190618769755185, 7.22078646875346e-17]), q:(0.9670887626187421, v:[0.010583037088333575, 0.25421904834714754, 7.37257477290143e-17]), q:(0.9659258262890683, v:[0.0, 0.25881904510252074, 0.0])] ===== ループ回数の決定 ===== test(100, lambda : interpolate_matrix(R1,R2,100)) 0.18225622177124023 test(1000, lambda : interpolate_matrix(R1,R2,100)) 1.597233772277832 test(10000, lambda : interpolate_matrix(R1,R2,100)) 18.435190200805664 test(100000, lambda : interpolate_matrix(R1,R2,100)) 177.21397614479065 内部で100分割の補間をしているので，計測の繰り返しは10000とする． N=10000 ===== 補間その１ ===== m_time = test(N, lambda : interpolate_matrix(R1,R2,100)) print(m_time) 17.621865272521973 q_time = test(N, lambda : interpolate_quaternion(q1,q2,100)) print(q_time) 11.456907510757446 data.append(judge('補間その１', m_time, q_time)) ===== 補間その２ ===== m_time = test(N, lambda : interpolate_matrix2(R1,R2,100)) print(m_time) 8.184473276138306 q_time = test(N, lambda : interpolate_quaternion2(q1,q2,100)) print(q_time) 7.437374830245972 data.append(judge('補間その２', m_time, q_time)) ===== 結論 ===== まとめの表 df = pd.DataFrame(data, columns=["項目", "MATRIX", "QUATERNION", "結果"]) df
項目 MATRIX QUATERNION 結果
0 補間その１ 17.621865 11.456908 QUATERNIONの勝ち
1 補間その２ 8.184473 7.437375 QUATERNIONの勝ち
予想通り回転の補間に関しては，QUATERNIONの方が速かった． 回転の合成（乗算）に関してQUATERNIONの方が速いのは元々わかっていたことではある． 直交行列の方はロドリゲス（回転軸と回転角度）からの生成に時間がかかるためさらに遅くなっている． 回転変換の生成をその都度行わなければ，速度の差はほぼ回転の合成の差にとどまっている． しかし，ロボットの軌道生成を考えた場合は，姿勢の補間だけではなく位置も補完する必要がある． さらにそこから逆運動学を解く必要もある． それらを行うとすれば直交行列で計算したほうが速いはずである． 上記のようなロボットの運動学や原点位置まで含めた座標変換を考えた場合に 四元数にどの程度の利点が残るかは私にはまだよくわかっていない．