座標系に関する演算プログラム

あらかじめベクトル，回転行列，座標系などのクラスとそれらの演算を用意することで 座標系を用いたロボットの動作生成や作業環境の記述などのプログラムが容易になる．

大規模なライブラリ用にこの手のツールが種々用意されているが 手元で手軽に使える自分用のツールは小回りが利いて実際にいろいろと便利である．

以下の4つのクラスと演算を実装する．

• ベクトル：和，差，絶対値，内積、スカラー積、外積、行列との積
• 直交行列：積、ベクトルとの積，列や行のベクトルとしての切り出し，オイラー角，回転軸と回転角の抽出．四元数への変換
• 四元数：回転変換としての四元数の生成．回転変換としての四元数同士の積．直交行列への変換
• 座標変換：座標系連鎖（積）、位置ベクトルとの積

これらの実装はpythonで行っているが，他の言語でも同様のクラスを作るのは容易であり，便利である．

ここでは簡単な問題を解きながら使い方を例示する．

• 基本的な使い方
• 直交行列と四元数の比較１
• 直交行列と四元数の比較２
• numpyとgeo.pyの速度比較

a=VECTOR(0.87, 0.5, 0)とb=VECTOR(0.71, 0.71, 0)との間の角度を求める。

例題1の解答

x=VECTOR(1,0,0)を基準に a=VECTOR(-0.707,-0.707,0)のz軸まわりの回転 角度を求める。

例題2の解答

a=VECTOR(1,1,0)をx軸まわりに90度回転させたベ クトルを求めよ。

ヒント：x軸周りに90度回転の回転行列を作り，a に左からかける．

例題3の解答

a=VECTOR(1,1,0)を回転させ、b=VECTOR(1,0,1)に 一致させるための、x軸以外の回転軸を1つとその まわりの回転角度をもとめよ。

ヒント：

• まずは回転軸を求める．どこにあるか．直観でも良い．
• 回転軸周りの回転角度は，例題2を参考にする．

例題4の解答

例題4で求めた回転軸、回転角度で a=VECTOR(1,1,0)を回転させ、 b=VECTOR(1,0,1)に 一致することを確認せよ。

例題5の解答

a=VECTOR(1,1,0)をx軸まわりに90度回転させたベ クトルを四元数を用いて求めよ。

例題6の解答

例題1 a=VECTOR(0.87, 0.5, 0)とb=VECTOR(0.71, 0.71, 0)との間の角度を求める。

>>> a=VECTOR(0.87,0.5,0)
>>> b=VECTOR(0.71,0.71,0)
>>> co=a.dot(b)/abs(a)/abs(b)
>>> th=acos(co)
>>> th
0.26377986630106198
>>> th*180/pi
15.113473059575982

例題2 x=VECTOR(1,0,0)を基準に a=VECTOR(-0.707,-0.707,0)のz軸まわりの回転 角度を求める。

>>> x=VECTOR(1,0,0)
>>> a=VECTOR(-0.707,-0.707,0)
>>> co=a.dot(x)/abs(a)
>>> th=acos(co)
>>> th
2.3561944901923448
>>> th*180/pi
135.0

>>> a=VECTOR(-0.707,-0.707,0)
>>> co=a.dot(VECTOR(1,0,0))
>>> si=a.dot(VECTOR(0,1,0))
>>> th=atan2(si,co)
>>> th*180/pi
-135.0

例題2の補足

例題3 a=VECTOR(1,1,0)をx軸まわりに90度回転させたベ クトルを求めよ。

>>> a=VECTOR(1,1,0)
>>> R=MATRIX(a=pi/2)
>>> b=R*a
>>> b
v:[1.0, 6.123233995736766e-17, 1.0]

例題4 a=VECTOR(1,1,0)を回転させ，b=VECTOR(1,0,1)に 一致させるための，x軸以外の回転軸を1つとその 回転角度をもとめよ．

>>> c=a*b
>>> c
v:[1.0, -1.0, -1.0]
>>> kco=a.dot(b)
>>> ksi=abs(c)
>>> th=atan2(ksi,kco)
>>> th
1.0471975511965976
>>> th*180/pi
59.999999999999993

回転軸はa+b, 回転角度はpi,

回転軸はa+bとa*bが張る平面上のどこでも良い． したがって， それぞれの比率をx,yとして関数を書くと，

>>> def foo(a,b,x,y) :
...    r1=a+b
...    r2=a*b
...    r=(x*r1+y*r2).normalize()
...    da=(r*a).normalize()
...    db=(r*b).normalize8)
...    dc=r*da
...    kco=da.dot(db)
...    ksi=dc.dot(db)
...    return(r,atan2(ksi,kco))
... 
>>> foo(a,b,1,0)
(v:[0.8164965809277261, 0.4082482904638631, 0.4082482904638631], 3.141592653589793)
>>> foo(a,b,0,1)
(v:[0.5773502691896258, -0.5773502691896258, -0.5773502691896258], 1.0471975511965979)
>>> foo(a,b,0,-1)
(v:[-0.5773502691896258, 0.5773502691896258, 0.5773502691896258], -1.0471975511965979)
>>> foo(a,b,1,1)
(v:[1.0, 0.0, 0.0], 1.5707963267948966)
>>>

例題5 例題4で求めた回転軸、回転角度で a=VECTOR(1,1,0)を回転させ、 b=VECTOR(1,0,1)に 一致することを確認せよ。

>>> tmp=foo(a,b,0,1)
>>> R=MATRIX(axis=tmp[0],angle=tmp[1])
>>> R*a
v:[1.0, -2.220446049250313e-16, 1.0000000000000002]
>>> 

例題6 a=VECTOR(1,1,0)をx軸まわりに90度回転させたベ クトルを四元数を用いて求めよ。

>>> a=VECTOR(1,1,0)
>>> qa=QUATERNION(w_v=[0,a])
>>> qr=(QUATERNION(a=pi/2))
>>> qb=qr*qa*qr.conjugate()
>>> qb
q:(0.0, v:[1.0, 2.220446049250313e-16, 1.0])
>>> b=qb.v
>>> b
v:[1.0, 2.220446049250313e-16, 1.0]
>>>