ロボットソフトウェア特論 (4-1)suehiro/class2016/presen/lecture...2016年度前学期...

ロボットソフトウェア特論 (4-1)

2016.5.11

電気通信大学

大学院情報理工学研究科

末廣尚士

電気通信大学大学院情報理工学研究科情報学専攻ロボットソフトウェア特論2016年度前学期

- MATRIX(座標回転行列)

3*3の行列

直交行列

3次元直交座標系の回転変換の表現

座標系の姿勢の表現

2



直交行列

𝐴T𝐴 = I 𝐴 = 𝒆x 𝒆y 𝒆z

𝐴T𝐴 =

𝒆xT

𝒆yT

𝒆zT

𝒆x 𝒆y 𝒆z =1 0 00 1 00 0 1

𝒆x ∙ 𝒆x = 1 𝒆y ∙ 𝒆y = 1 𝒆z ∙ 𝒆z = 1

𝒆x ∙ 𝒆y = 0 𝒆y ∙ 𝒆z = 0 𝒆z ∙ 𝒆x = 0

注： 𝒂T𝒃 = 𝒂 ∙ 𝒃3



3次元直交座標系の回転変換の表現

座標系の姿勢の表現

𝐴 = 𝒆x 𝒆y 𝒆z

4

注：ここでは原点の位置は考えない



回転変換の逆変換

5

𝐴 = 𝒆x 𝒆y 𝒆z

𝐴T

注：ここでは原点の位置は考えない


- 座標系の回転座標系の回転はその姿勢の変化になる。

たとえばｘ軸回りの回転変換Rx 𝜃 は以下のようになる。

𝒆x =100

𝒆y =0

cos 𝜃sin 𝜃

𝒆z =0

−sin 𝜃cos 𝜃

𝑅x 𝜃 =100

0cos 𝜃sin 𝜃

0− sin 𝜃cos 𝜃

y, z軸まわりの回転も同様に，

𝑅y 𝜃 =cos 𝜃0

− sin 𝜃

010

sin 𝜃0

cos 𝜃𝑅z 𝜃 =

cos 𝜃sin 𝜃0

− sin 𝜃cos 𝜃0

001

6


- 姿勢の表現についての補足

姿勢変換行列

オイラー角による表現回転軸の異なる３つ回転で表現。何通りもある。

物理学では、z, x ,zがよく用いられる。

乗り物では、ロール(roll)、ピッチ(pitch)、ヨウ(yaw)

ロボットでは、𝛼, 𝛽, 𝛾（軸の取り方は色々）

ロール、ピッチ、ヨウと𝛼, 𝛽, 𝛾は基本的に同じ（ような）もの

その他、回転軸ベクトル(Rodrigues)，四元数(Quaternion)など

7


- 𝛼, 𝛽, 𝛾の表現3つの座標軸のまわりに順に回転させることで姿勢変換を表現する。

基本的にはz軸を進行方向としたヨウ、ピッチ、ロールと同等

x, y, z軸の順に𝛼, 𝛽, 𝛾回転(順番が異なる場合もある)

8


- 𝛼, 𝛽, 𝛾の変換行列

9

𝑅x(𝛼)=100

0cos 𝛼sin 𝛼

0− sin𝛼cos 𝛼

𝑅y 𝛽 =cos𝛽0

− sin𝛽

010

sin𝛽0

cos 𝛽𝑅z 𝛾 =

cos 𝛾sin 𝛾0

− sin 𝛾cos 𝛾0

001

𝑅x(𝛼) 𝑅y 𝛽 𝑅z 𝛾 =

100

0cos 𝛼sin𝛼

0− sin𝛼cos 𝛼

cos 𝛽0

− sin𝛽

010

sin𝛽0

cos 𝛽

cos 𝛾sin 𝛾0

−sin 𝛾cos 𝛾0

001


- 𝛼, 𝛽, 𝛾の変換行列(つづき)

10

𝑅x(𝛼) 𝑅y 𝛽 𝑅z 𝛾 =

100

0cos 𝛼sin𝛼

0−sin𝛼cos 𝛼

cos 𝛽0

− sin𝛽

010

sin 𝛽0

cos 𝛽

cos 𝛾sin 𝛾0

−sin 𝛾cos 𝛾0

001

=

cos𝛽sin𝛼 sin𝛽

− cos𝛼 sin𝛽

0cos 𝛼sin 𝛼

sin𝛽−sin𝛼 cos 𝛽cos 𝛼 cos 𝛽

cos 𝛾sin 𝛾0

− sin 𝛾cos 𝛾0

001

=

c 𝛽 c 𝛾

s 𝛼 s 𝛽 c 𝛾 + c 𝛼 s(γ)

−c 𝛼 s 𝛽 c 𝛾 + s 𝛼 s 𝛾

− c 𝛽 s 𝛾

− s 𝛼 s 𝛽 s 𝛾 + c 𝛼 c(γ)

c 𝛼 s 𝛽 s 𝛾 + s 𝛼 c 𝛾

s(𝛽)

− s 𝛼 c 𝛽

𝑐 𝛼 c 𝛽


- 回転行列の𝛼, 𝛽, 𝛾

逆に回転行列Aが与えられたときに𝛼, 𝛽, 𝛾を求める

𝐴 =

𝑎xx 𝑎xy 𝑎xz𝑎yx 𝑎yy 𝑎yz𝑎zx 𝑎zy 𝑎zz

係数を比較すると 𝑎xz = sin 𝛽

これから− Τ𝜋 2 < 𝛽 ≤ Τ𝜋 2を考慮すると𝛽 = sin−1 𝑎xzまた− Τ𝑎yz 𝑎zz = Τsin 𝛼 cos 𝛽 cos 𝛼 cos 𝛽 = Τsin 𝛼 cos 𝛼

cos 𝛽 ≥ 0 なので cos 𝛽 ≠ 0 のとき，𝛼 = atan2 −𝑎yz , 𝑎zz

同様に𝛾 = atan2 −𝑎xy , 𝑎xx

注： cos 𝛽 = 0 のとき𝛼, 𝛾 は不定となる．

11


- 回転行列の特異点

cos 𝛽 = 0, sin𝛽 = 1 のとき

𝐴 =


=

0s 𝛼 c 𝛾 + c 𝛼 s(γ)

− c 𝛼 c 𝛾 + s 𝛼 s 𝛾

0− s 𝛼 s 𝛾 + c 𝛼 c(γ)

c 𝛼 s 𝛾 + s 𝛼 c 𝛾

100

=

0 0 1sin 𝛼 + 𝛾 cos 𝛼 + 𝛾 0

−cos 𝛼 + 𝛾 sin 𝛼 + 𝛾 0

よって 𝛽 = Τ𝜋 2 ，α + 𝛾 = atan2 𝑎yx , 𝑎yy

同様にcos 𝛽 = 0, sin 𝛽 = −1 のとき

𝛽 = Τ−𝜋 2 ，α − 𝛾 = −atan2 𝑎yx , 𝑎yy

12


- ロール、ピッチ、ヨウの表現

乗り物（たとえば船、飛行機、ロケット）の姿勢を表現する。

乗り物本来の姿勢（乗り物の座標系）は進行方向や水平面（乗り物の外の座標系）からずれることがある。

そのずれを表現するのがロール、ピッチ、ヨウ

13


- rpyまたは𝛼, 𝛽, 𝛾の表現の特徴

𝛽(ピッチ)が± Τ𝜋 2 になると、𝛼(ヨウ)と𝛾(ロール)の区別が付かなくなる

𝛼,𝛾が±𝜋を、𝛽が± Τ𝜋 2を超えると、表現が１つに定まらなくなる(周期性があり範囲を限ると切り替えのときに不連続が生じる)。

14


- 回転軸ベクトル表現

姿勢変換行列を1つ回転軸𝒏およびその回転角𝜃で表現する。

姿勢を考慮した移動軌道の生成に良く用いる

利点： 1回の回転で実現できる

欠点：人間には分かりにくい

0と𝜋 が特異点になる。

15


- 回転軸まわりの回転(1)

回転軸𝒏まわりに𝜃回転させたときの変換行列を求める(ロドリゲスの回転公式)．

回転軸ベクトル：𝒏 =

𝑛x𝑛y𝑛z

回転行列：𝑅 𝒏, 𝜃 = 𝒆x 𝒆y 𝒆z

ここで 0 ≤ 𝜃 ≤ 𝜋 とする

16



x軸𝒙を回転軸𝒏まわりに𝜃回転させた𝒆xを求める。

図のように𝒙を𝒏に垂直なベクトル𝒓と平行なベクトル𝒅に分解する

𝒙 = 𝒓 + 𝒅

ここで 𝒅 = 𝒙 ∙ 𝒏 𝒏 = 𝑛x𝒏

また 𝒓 = 𝒙 − 𝒅 = 𝒙 − 𝑛x𝒏

𝒓 を回転したベクトルを𝒓′とすると

𝒆x = 𝒓′ + 𝒅

17



𝒓 を回転したベクトル𝒓′

図のように，

𝒓′ = 𝒓 cos 𝜃 + 𝒏 × 𝒓 sin 𝜃

ここで，

𝒓 = 𝒙 − 𝑛x𝒏

𝒏 × 𝒓 = 𝒏 × 𝒙 − 𝑛x𝒏 = 𝒏 × 𝒙

よって，

𝒆x = 𝒅 + 𝒓′ = 𝑛x𝒏 + 𝒙 − 𝑛x𝒏 cos 𝜃 + 𝒏 × 𝒙 sin 𝜃

18


- 回転軸まわりの回転(4)𝒆x = 𝑛x𝒏 + 𝒙 − 𝑛x𝒏 cos 𝜃 + 𝒏 × 𝒙 sin 𝜃

𝒏 =

𝑛x𝑛y𝑛z

，𝒙 =100を代入する

𝒆x = 𝑛x

𝑛x𝑛y𝑛z

+100

− 𝑛x

𝑛x𝑛y𝑛z

cos 𝜃 +

0𝑛z−𝑛y

sin 𝜃

整理して 𝒆x =

cos 𝜃 + 𝑛x2 1 − cos 𝜃

𝑛x𝑛y 1 − cos 𝜃 + 𝑛z sin 𝜃

𝑛z𝑛x 1 − cos 𝜃 − 𝑛y sin 𝜃19


- 補足

𝒏 × 𝒙 = 𝑛x𝒙 + 𝑛y𝒚 + 𝑛z𝒛 × 𝒙

= 𝑛x 𝒙 × 𝒙 + 𝑛y 𝒚 × 𝒙 + 𝑛x 𝒛 × 𝒙

= −𝑛y𝒛 + 𝑛z𝒚 =

0𝑛z−𝑛y

20


- 回転軸まわりの回転(5)𝒆y = 𝑛y𝒏 + 𝒚 − 𝑛y𝒏 cos 𝜃 + 𝒏 × 𝒚 sin 𝜃

𝒆z = 𝑛z𝒏 + 𝒛 − 𝑛z𝒏 cos 𝜃 + 𝒏 × 𝒛 sin 𝜃

𝒆y =

𝑛x𝑛y 1 − cos 𝜃 − 𝑛z sin 𝜃

cos 𝜃 + 𝑛y2 1 − cos 𝜃

𝑛y𝑛z 1 − cos 𝜃 + 𝑛x sin 𝜃

𝒆z =

𝑛z𝑛x 1 − cos 𝜃 + 𝑛y sin 𝜃

𝑛y𝑛z 1 − cos 𝜃 − 𝑛x sin 𝜃

cos 𝜃 + 𝑛z2 1 − cos 𝜃

21



回転軸𝒏まわりに𝜃回転させたときの変換行列

𝒏 =

𝑛x𝑛y𝑛z

， 0 ≤ 𝜃 ≤ 𝜋

𝑅 𝒏, 𝜃 = 𝒆x 𝒆y 𝒆z =

cos 𝜃 + 𝑛x2 1 − cos 𝜃

𝑛x𝑛y 1 − cos 𝜃 + 𝑛z sin 𝜃

𝑛z𝑛x 1 − cos 𝜃 − 𝑛y sin 𝜃

𝑛x𝑛y 1 − cos𝜃 − 𝑛z sin 𝜃

cos 𝜃 + 𝑛y2 1 − cos 𝜃

𝑛y𝑛z 1 − cos 𝜃 + 𝑛x sin 𝜃

𝑛z𝑛x 1 − cos 𝜃 + 𝑛y sin 𝜃

𝑛y𝑛z 1 − cos 𝜃 − 𝑛x sin 𝜃

cos 𝜃 + 𝑛z2 1 − cos 𝜃

22


- 回転行列の回転軸、回転角(1)逆に回転行列𝐴が与えられたときに回転軸𝒏，回転角度𝜃を求める

𝐴 =


係数を比較して，

𝑎xx + 𝑎yy + 𝑎zz

= cos 𝜃 + 𝑛x2 1 − cos 𝜃 + cos 𝜃 + 𝑛z

2 1 − cos 𝜃 + cos 𝜃 + 𝑛z2 1 − cos 𝜃

= 3 cos 𝜃 + 𝑛x2 + 𝑛y

2 + 𝑛z2 1 − cos 𝜃

= 1 + 2 cos 𝜃

よって

cos 𝜃 = Τ𝑎xx + 𝑎yy + 𝑎zz − 1 2

𝜃 = cos−1 Τ𝑎xx + 𝑎yy + 𝑎zz − 1 2 ， 0 ≤ 𝜃 ≤ 𝜋

23


- 回転行列の回転軸、回転角(2)𝑎yz − 𝑎zy

= 𝑛y𝑛z 1 − cos 𝜃 − 𝑛x sin 𝜃 − 𝑛y𝑛z 1 − cos 𝜃 + 𝑛x sin 𝜃

= −2𝑛x sin 𝜃

よって

𝑛x = − Τ𝑎yz − 𝑎zy 2 sin 𝜃

同様に

𝑛y = − Τ𝑎zx − 𝑎xz 2 sin 𝜃

𝑛z = − Τ𝑎xy − 𝑎yx 2 sin 𝜃

注：実際には 2 sin 𝜃で割らずに正規化するのがよい。

24


- 回転行列とベクトルの積

𝐴𝒗 の解釈

𝐴 = 𝒆x 𝒆y 𝒆z ，𝒗 =

𝑣x𝑣y𝑣z

𝐴𝒗 = 𝒆x𝑣x + 𝒆y𝑣y + 𝒆z𝑣z

25



𝐴𝒗 の解釈


𝑣x𝑣y𝑣z


26



𝐴𝒗 の解釈


𝑣x𝑣y𝑣z


27


- 回転行列とベクトルの積解釈1：座標変換座標系1で表現されたベクトル 0𝒗は𝐴を左から掛けることにより元の座

標系0での表現に変換される．

𝐴 = 𝒆x 𝒆y 𝒆z ， 0𝒗 =

0𝑣x0𝑣y0𝑣z

， 1𝒗 =

1𝑣x1𝑣y1𝑣z

0𝒗 = 𝐴 1𝒗 = 𝒆x1𝑣x + 𝒆y

1𝑣y + 𝒆z1𝑣z

28


- 回転行列とベクトルの積解釈1：ベクトルの回転座標系0で表現されていた𝒗を座標系ごと回転させて元の座標系0の表現に戻すことにより，回転したベクトル𝒗′の表現を得る．


𝑣x𝑣y𝑣z

，𝒗′ =

𝑣x′

𝑣y′

𝑣z′

𝑣′ = 𝐴𝑣 = 𝑒x𝑣x + 𝑒y𝑣y + 𝑒z𝑣z

29


- 三次元回転行列のプログラム

行列の構造生成方法

値の参照，変更

行，列のベクトル化

行列の表示

行列の演算（関数，メソッド）行列の積

行列とベクトルの積

逆行列（=転置行列）

回転軸，回転角

𝛼, 𝛽, 𝛾 表現

30


- MATRIX(座標回転行列)のデータ構造

リストのサブクラス

行、列の構造：[行1, 行2, 行3] m=[[...],[...],[...]]

m[i][j]はmのi行j列要素

列ベクトルのリストとする考え方もあるが，それでは添え字の扱いが一般の行列と異なってしまう．

31


- MATRIXのクラス定義

MATRIXをlistのサブクラスとして定義

いろいろな生成方法をサポート引数なし => 単位行列

行列（またはリストのリスト）mat => コピー

x軸まわりの回転角a => x軸まわりの回転行列

y...

z...

回転軸axis,回転角angle => 回転行列

32

class MATRIX(list) :def __init__(self, mat=[], a=0.0, b=0.0, c=0.0, angle=0.0, axis=[]) :


- MATRIXの生成

33

class MATRIX(list) :def __init__(self, mat=[], a=0.0, b=0.0, c=0.0, angle=0.0, axis=[]) :list.__init__(self)self.append([1.0, 0.0, 0.0])self.append([0.0, 1.0, 0.0])self.append([0.0, 0.0, 1.0])if mat :for i in [0,1,2] :for j in [0,1,2] :self[i][j] = mat[i][j]

elif a != 0.0 :self[0][0] = 1.0;self[1][1] = cos(a);self[2][2] = cos(a);self[1][2] = -sin(a);self[2][1] = sin(a);

elif b != 0.0 :..

引数無し

行列コピー

x軸回転

y軸回転


- MATRIXの行、列のベクトル化

各行、列をベクトルとして取り出す。

34

def col(self, idx, arg=[]) :tmp = VECTOR()if arg :for i in [0,1,2] :self[i][idx] = arg[i]

for i in [0,1,2] :tmp[i] = self[i][idx]

return(tmp)def row(self, idx, arg=[]) :tmp = VECTOR()if arg :for i in [0,1,2] :self[idx][i] = arg[i]

for i in [0,1,2] :tmp[i] = self[idx][i]

return(tmp)

列のベクトル化

行のベクトル化


- MATRIXの表示

先頭にm:を付ける

35

def __repr__(self) :return("m:" + list.__repr__(self))


- MATRIXの積：「*」

行列同士，行列とベクトル

36

def __mul__(self, other) :tmp = Noneif isinstance(other, MATRIX) :tmp = MATRIX()for i in [0,1,2] :for j in [0,1,2] :tmp[i][j] = (self[i][0] * other[0][j] +

self[i][1] * other[1][j] +self[i][2] * other[2][j])

elif isinstance(other, VECTOR) :tmp = VECTOR()for i in [0,1,2] :tmp[i] = (self[i][0] * other[0] +

self[i][1] * other[1] +self[i][2] * other[2])

return(tmp)


- MATRIXの転置と逆行列

座標回転行列の場合は転置と逆行列が同じになる。

𝐴 = 𝒆x 𝒆y 𝒆z 𝐴T =

𝒆xT

𝒆yT

𝒆zT

𝐴T𝐴 = I つまり 𝐴T = 𝐴−1

37


- 補足：一般の3x3の逆行列

𝐴 = 𝒂 𝒃 𝒄

𝐴−1 =

𝒃×𝒄

𝒂⋅ 𝒃×𝒄

T

𝒄×𝒂

𝒃⋅ 𝒄×𝒂

T

𝒂×𝒃

𝒄⋅ 𝒂×𝒃

T

𝑎 ⋅𝒃×𝒄

𝒂⋅ 𝒃×𝒄= 1, 𝑎 ⋅

𝒄×𝒂

𝒃⋅ 𝒄×𝒂= 0, 𝑎 ⋅

𝒂×𝒃

𝒄⋅ 𝒂×𝒃= 0

38


- 補足：ベクトル3重積

𝒂 ⋅ 𝒃 × 𝒄 = 𝒂 𝒃 𝒄 cos𝜙 sin 𝜃

これは𝒂，𝒃，𝒄 を辺とする平行6面体の体積

よって𝒂 ⋅ 𝒃 × 𝒄 = 𝒃 ⋅ 𝒄 × 𝒂 = 𝒄 ⋅ 𝒂 × 𝒃となることがわかる．(外積の順序に注意)

𝒂 ⋅ 𝒃 × 𝒄 = 0の場合逆行列は存在しない．

39


- MATRIXの転置と逆行列

転置： A.trans()

逆行列： - A (今回は直行行列なので転置でごまかす)

40

def trans(self) :tmp = MATRIX()for i in [0,1,2] :for j in [0,1,2] :tmp[i][j] = self[j][i]

return(tmp)def __neg__(self) :return(self.trans())


- MATRIXの回転角、回転軸回転角、回転軸：angle,axis=A.rot_axis()

41

def rot_axis(self) :axis = VECTOR()co=(self[0][0]+self[1][1]+self[2][2]-1.0)/2.0;if co <= -1.0 :angl = pi;axis[0] = sqrt((self[0][0] + 1.0)/2.0)axis[1] = sign(self[0][1])*sqrt((self[1][1]+1.0)/2.0)axis[2] = sign(self[0][2])*sqrt((self[2][2]+1.0)/2.0)

elif co < 1.0 :axis[0] = self[2][1] - self[1][2]axis[1] = self[0][2] - self[2][0]axis[2] = self[1][0] - self[0][1]an = abs(axis)if(an != 0.0) :for i in [0,1,2] :axis[i] = axis[i]/ansi = an/2.0

angl = arctan2(si,co);


- MATRIXの回転角、回転軸

42

else :angl = 0.0axis[0] = 1.0axis[1] = 0.0axis[2] = 0.0

return([angl,axis])


- MATRIXの𝛼, 𝛽, 𝛾

𝛼, 𝛽, 𝛾：[a, b, c] =A.abc()

43

def abc(self) :if self[0][2]>= 1.0 :b = pi/2;a = 0.0;c = atan2(self[2][1],self[1][1])

elif self[0][2] <= -1.0 :b = -pi/2;a = 0.0;c = atan2(self[1][0],self[2][0])

else :b = asin(self[0][2]);a = atan2(- self[1][2],self[2][2]);c = atan2(- self[0][1],self[0][0]);

return([a, b, c])


- ベクトルの生成例

44

x軸回りの回転角を指定して生成

回転軸と回転角を指定して生成．

>>> a=MATRIX(a=pi/3)>>> am:[[1.0, 0.0, 0.0], [0.0, 0.5000000000000001, -0.8660254037844386], [0.0, 0.8660254037844386, 0.5000000000000001]]

>>> b=MATRIX(axis=VECTOR(1,1,0),angle=pi/4)>>> bm:[[0.8535533905932737, 0.14644660940672619, 0.4999999999999999], [0.14644660940672619, 0.8535533905932737, -0.4999999999999999], [-0.49999999999999990.5000000000000001]], 0.4999999999999999, 0.7071067811865476]]


- ベクトル化の使用例

45

>>> a.row(1)v:[0.0, 0.5000000000000001, -0.8660254037844386]>>> a.col(1)v:[0.0, 0.5000000000000001, 0.8660254037844386]


- 積，逆行列，転置の使用例(1)

46

>>> b*(-b)m:[[0.9999999999999999, 0.0, 5.551115123125783e-17], [0.0, 0.9999999999999999, -5.551115123125783e-17], [5.551115123125783e-17, -5.551115123125783e-17, 0.9999999999999999]]>>> b*b.trans()m:[[0.9999999999999999, 0.0, 5.551115123125783e-17], [0.0, 0.9999999999999999, -5.551115123125783e-17], [5.551115123125783e-17, -5.551115123125783e-17, 0.9999999999999999]]


- 積，逆行列，転置の使用例(2)

47

>>> c=VECTOR(1,0,0)>>> d=b*c>>> dv:[0.8535533905932737, 0.14644660940672619, -0.4999999999999999]>>> (-b)*dv:[0.9999999999999999, 0.0, -5.551115123125783e-17]


- 回転角，回転軸，𝛼, 𝛽, 𝛾の使用例

48

>>> a.abc()[1.0471975511965976, 0.0, -0.0]>>> a.rot_axis()[1.0471975511965976, v:[1.0, 0.0, 0.0]]>>> b.rot_axis()[0.7853981633974483, v:[0.7071067811865476, 0.7071067811865476, 0.0]]>>> b.abc()[0.6154797086703871, 0.5235987755982987, -0.1699184547270609]


- 例題5-3

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

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

49


- 例題5-3の答え

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

50

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


- 例題5-4

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

ヒント：まずは回転軸を求める．どこにあるか．直観でも良い．

回転軸周りの回転角度は，例題5-2を参考にする．

51


- 例題5-4の答えの例

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

52

>>> c=a*b>>> cv:[1.0, -1.0, -1.0]>>> kco=a.dot(b)>>> ksi=abs(c)>>> th=atan2(ksi,kco)>>> th1.0471975511965976>>> th*180/pi59.999999999999993


- 例題5-5

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

53


- 例題5-5の答えの例

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

54

>>> R2=MATRIX(axis=c,angle=th)>>> d=R2*a>>> dv:[1.0, 0.0, 1.0]


次回予告

座標系表現(FRAME)

python-visual (Vpython)

Vpythonをインストールしておくこと

55

ロボットソフトウェア特論 (4-1)suehiro/class2016/presen/lecture...2016年度前学期...

Documents