Upload
others
View
5
Download
0
Embed Size (px)
Citation preview
ロボットソフトウェア特論 (4-1)
2016.5.11
電気通信大学
大学院情報理工学研究科
末廣尚士
電気通信大学大学院情報理工学研究科情報学専攻 ロボットソフトウェア特論2016年度前学期
- MATRIX(座標回転行列)
3*3の行列
直交行列
3次元直交座標系の回転変換の表現
座標系の姿勢の表現
2
電気通信大学大学院情報理工学研究科情報学専攻 ロボットソフトウェア特論2016年度前学期
- MATRIX(座標回転行列)
直交行列
𝐴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
電気通信大学大学院情報理工学研究科情報学専攻 ロボットソフトウェア特論2016年度前学期
- MATRIX(座標回転行列)
3次元直交座標系の回転変換の表現
座標系の姿勢の表現
𝐴 = 𝒆x 𝒆y 𝒆z
4
注:ここでは原点の位置は考えない
電気通信大学大学院情報理工学研究科情報学専攻 ロボットソフトウェア特論2016年度前学期
- MATRIX(座標回転行列)
回転変換の逆変換
5
𝐴 = 𝒆x 𝒆y 𝒆z
𝐴T
注:ここでは原点の位置は考えない
電気通信大学大学院情報理工学研究科情報学専攻 ロボットソフトウェア特論2016年度前学期
- 座標系の回転座標系の回転はその姿勢の変化になる。
たとえばx軸回りの回転変換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
電気通信大学大学院情報理工学研究科情報学専攻 ロボットソフトウェア特論2016年度前学期
- 姿勢の表現についての補足
姿勢変換行列
オイラー角による表現 回転軸の異なる3つ回転で表現。何通りもある。
物理学では、z, x ,zがよく用いられる。
乗り物では、ロール(roll)、ピッチ(pitch)、ヨウ(yaw)
ロボットでは、𝛼, 𝛽, 𝛾(軸の取り方は色々)
ロール、ピッチ、ヨウと𝛼, 𝛽, 𝛾は基本的に同じ(ような)もの
その他、回転軸ベクトル(Rodrigues),四元数(Quaternion)など
7
電気通信大学大学院情報理工学研究科情報学専攻 ロボットソフトウェア特論2016年度前学期
- 𝛼, 𝛽, 𝛾の表現3つの座標軸のまわりに順に回転させることで姿勢変換を表現する。
基本的にはz軸を進行方向としたヨウ、ピッチ、ロールと同等
x, y, z軸の順に𝛼, 𝛽, 𝛾回転(順番が異なる場合もある)
8
電気通信大学大学院情報理工学研究科情報学専攻 ロボットソフトウェア特論2016年度前学期
- 𝛼, 𝛽, 𝛾の変換行列
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
電気通信大学大学院情報理工学研究科情報学専攻 ロボットソフトウェア特論2016年度前学期
- 𝛼, 𝛽, 𝛾の変換行列(つづき)
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 𝛽
電気通信大学大学院情報理工学研究科情報学専攻 ロボットソフトウェア特論2016年度前学期
- 回転行列の𝛼, 𝛽, 𝛾
逆に回転行列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
電気通信大学大学院情報理工学研究科情報学専攻 ロボットソフトウェア特論2016年度前学期
- 回転行列の特異点
cos 𝛽 = 0, sin𝛽 = 1 のとき
𝐴 =
𝑎xx 𝑎xy 𝑎xz𝑎yx 𝑎yy 𝑎yz𝑎zx 𝑎zy 𝑎zz
=
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
電気通信大学大学院情報理工学研究科情報学専攻 ロボットソフトウェア特論2016年度前学期
- ロール、ピッチ、ヨウの表現
乗り物(たとえば船、飛行機、ロケット)の姿勢を表現する。
乗り物本来の姿勢(乗り物の座標系)は進行方向や水平面(乗り物の外の座標系)からずれることがある。
そのずれを表現するのがロール、ピッチ、ヨウ
13
電気通信大学大学院情報理工学研究科情報学専攻 ロボットソフトウェア特論2016年度前学期
- rpyまたは𝛼, 𝛽, 𝛾の表現の特徴
𝛽(ピッチ)が± Τ𝜋 2 になると、𝛼(ヨウ)と𝛾(ロール)の区別が付かなくなる
𝛼,𝛾が±𝜋を、𝛽が± Τ𝜋 2を超えると、表現が1つに定まらなくなる(周期性があり範囲を限ると切り替えのときに不連続が生じる)。
14
電気通信大学大学院情報理工学研究科情報学専攻 ロボットソフトウェア特論2016年度前学期
- 回転軸ベクトル表現
姿勢変換行列を1つ回転軸𝒏およびその回転角𝜃で表現する。
姿勢を考慮した移動軌道の生成に良く用いる
利点: 1回の回転で実現できる
欠点: 人間には分かりにくい
0と𝜋 が特異点になる。
15
電気通信大学大学院情報理工学研究科情報学専攻 ロボットソフトウェア特論2016年度前学期
- 回転軸まわりの回転(1)
回転軸𝒏まわりに𝜃回転させたときの変換行列を求める(ロドリゲスの回転公式).
回転軸ベクトル:𝒏 =
𝑛x𝑛y𝑛z
回転行列:𝑅 𝒏, 𝜃 = 𝒆x 𝒆y 𝒆z
ここで 0 ≤ 𝜃 ≤ 𝜋 とする
16
電気通信大学大学院情報理工学研究科情報学専攻 ロボットソフトウェア特論2016年度前学期
- 回転軸まわりの回転(2)
x軸𝒙を回転軸𝒏まわりに𝜃回転させた𝒆xを求める。
図のように𝒙を𝒏に垂直なベクトル𝒓と平行なベクトル𝒅に分解する
𝒙 = 𝒓 + 𝒅
ここで 𝒅 = 𝒙 ∙ 𝒏 𝒏 = 𝑛x𝒏
また 𝒓 = 𝒙 − 𝒅 = 𝒙 − 𝑛x𝒏
𝒓 を回転したベクトルを𝒓′とすると
𝒆x = 𝒓′ + 𝒅
17
電気通信大学大学院情報理工学研究科情報学専攻 ロボットソフトウェア特論2016年度前学期
- 回転軸まわりの回転(3)
𝒓 を回転したベクトル𝒓′
図のように,
𝒓′ = 𝒓 cos 𝜃 + 𝒏 × 𝒓 sin 𝜃
ここで,
𝒓 = 𝒙 − 𝑛x𝒏
𝒏 × 𝒓 = 𝒏 × 𝒙 − 𝑛x𝒏 = 𝒏 × 𝒙
よって,
𝒆x = 𝒅 + 𝒓′ = 𝑛x𝒏 + 𝒙 − 𝑛x𝒏 cos 𝜃 + 𝒏 × 𝒙 sin 𝜃
18
電気通信大学大学院情報理工学研究科情報学専攻 ロボットソフトウェア特論2016年度前学期
- 回転軸まわりの回転(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
電気通信大学大学院情報理工学研究科情報学専攻 ロボットソフトウェア特論2016年度前学期
- 補足
𝒏 × 𝒙 = 𝑛x𝒙 + 𝑛y𝒚 + 𝑛z𝒛 × 𝒙
= 𝑛x 𝒙 × 𝒙 + 𝑛y 𝒚 × 𝒙 + 𝑛x 𝒛 × 𝒙
= −𝑛y𝒛 + 𝑛z𝒚 =
0𝑛z−𝑛y
20
電気通信大学大学院情報理工学研究科情報学専攻 ロボットソフトウェア特論2016年度前学期
- 回転軸まわりの回転(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
電気通信大学大学院情報理工学研究科情報学専攻 ロボットソフトウェア特論2016年度前学期
- 回転軸まわりの回転(6)
回転軸𝒏まわりに𝜃回転させたときの変換行列
𝒏 =
𝑛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
電気通信大学大学院情報理工学研究科情報学専攻 ロボットソフトウェア特論2016年度前学期
- 回転行列の回転軸、回転角(1)逆に回転行列𝐴が与えられたときに回転軸𝒏,回転角度𝜃を求める
𝐴 =
𝑎xx 𝑎xy 𝑎xz𝑎yx 𝑎yy 𝑎yz𝑎zx 𝑎zy 𝑎zz
係数を比較して,
𝑎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
電気通信大学大学院情報理工学研究科情報学専攻 ロボットソフトウェア特論2016年度前学期
- 回転行列の回転軸、回転角(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
電気通信大学大学院情報理工学研究科情報学専攻 ロボットソフトウェア特論2016年度前学期
- 回転行列とベクトルの積
𝐴𝒗 の解釈
𝐴 = 𝒆x 𝒆y 𝒆z ,𝒗 =
𝑣x𝑣y𝑣z
𝐴𝒗 = 𝒆x𝑣x + 𝒆y𝑣y + 𝒆z𝑣z
25
電気通信大学大学院情報理工学研究科情報学専攻 ロボットソフトウェア特論2016年度前学期
- 回転行列とベクトルの積
𝐴𝒗 の解釈
𝐴 = 𝒆x 𝒆y 𝒆z ,𝒗 =
𝑣x𝑣y𝑣z
𝐴𝒗 = 𝒆x𝑣x + 𝒆y𝑣y + 𝒆z𝑣z
26
電気通信大学大学院情報理工学研究科情報学専攻 ロボットソフトウェア特論2016年度前学期
- 回転行列とベクトルの積
𝐴𝒗 の解釈
𝐴 = 𝒆x 𝒆y 𝒆z ,𝒗 =
𝑣x𝑣y𝑣z
𝐴𝒗 = 𝒆x𝑣x + 𝒆y𝑣y + 𝒆z𝑣z
27
電気通信大学大学院情報理工学研究科情報学専攻 ロボットソフトウェア特論2016年度前学期
- 回転行列とベクトルの積解釈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
電気通信大学大学院情報理工学研究科情報学専攻 ロボットソフトウェア特論2016年度前学期
- 回転行列とベクトルの積解釈1:ベクトルの回転座標系0で表現されていた𝒗を座標系ごと回転させて元の座標系0の表現に戻すことにより,回転したベクトル𝒗′の表現を得る.
𝐴 = 𝒆x 𝒆y 𝒆z ,𝒗 =
𝑣x𝑣y𝑣z
,𝒗′ =
𝑣x′
𝑣y′
𝑣z′
𝑣′ = 𝐴𝑣 = 𝑒x𝑣x + 𝑒y𝑣y + 𝑒z𝑣z
29
電気通信大学大学院情報理工学研究科情報学専攻 ロボットソフトウェア特論2016年度前学期
- 三次元回転行列のプログラム
行列の構造 生成方法
値の参照,変更
行,列のベクトル化
行列の表示
行列の演算(関数,メソッド) 行列の積
行列とベクトルの積
逆行列(=転置行列)
回転軸,回転角
𝛼, 𝛽, 𝛾 表現
30
電気通信大学大学院情報理工学研究科情報学専攻 ロボットソフトウェア特論2016年度前学期
- MATRIX(座標回転行列)のデータ構造
リストのサブクラス
行、列の構造:[行1, 行2, 行3] m=[[...],[...],[...]]
m[i][j]はmのi行j列要素
列ベクトルのリストとする考え方もあるが,それでは添え字の扱いが一般の行列と異なってしまう.
31
電気通信大学大学院情報理工学研究科情報学専攻 ロボットソフトウェア特論2016年度前学期
- 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=[]) :
電気通信大学大学院情報理工学研究科情報学専攻 ロボットソフトウェア特論2016年度前学期
- 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軸回転
電気通信大学大学院情報理工学研究科情報学専攻 ロボットソフトウェア特論2016年度前学期
- 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)
列のベクトル化
行のベクトル化
電気通信大学大学院情報理工学研究科情報学専攻 ロボットソフトウェア特論2016年度前学期
- MATRIXの表示
先頭にm:を付ける
35
def __repr__(self) :return("m:" + list.__repr__(self))
電気通信大学大学院情報理工学研究科情報学専攻 ロボットソフトウェア特論2016年度前学期
- 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)
電気通信大学大学院情報理工学研究科情報学専攻 ロボットソフトウェア特論2016年度前学期
- MATRIXの転置と逆行列
座標回転行列の場合は転置と逆行列が同じになる。
𝐴 = 𝒆x 𝒆y 𝒆z 𝐴T =
𝒆xT
𝒆yT
𝒆zT
𝐴T𝐴 = I つまり 𝐴T = 𝐴−1
37
電気通信大学大学院情報理工学研究科情報学専攻 ロボットソフトウェア特論2016年度前学期
- 補足:一般の3x3の逆行列
𝐴 = 𝒂 𝒃 𝒄
𝐴−1 =
𝒃×𝒄
𝒂⋅ 𝒃×𝒄
T
𝒄×𝒂
𝒃⋅ 𝒄×𝒂
T
𝒂×𝒃
𝒄⋅ 𝒂×𝒃
T
𝑎 ⋅𝒃×𝒄
𝒂⋅ 𝒃×𝒄= 1, 𝑎 ⋅
𝒄×𝒂
𝒃⋅ 𝒄×𝒂= 0, 𝑎 ⋅
𝒂×𝒃
𝒄⋅ 𝒂×𝒃= 0
38
電気通信大学大学院情報理工学研究科情報学専攻 ロボットソフトウェア特論2016年度前学期
- 補足: ベクトル3重積
𝒂 ⋅ 𝒃 × 𝒄 = 𝒂 𝒃 𝒄 cos𝜙 sin 𝜃
これは𝒂,𝒃,𝒄 を辺とする平行6面体の体積
よって𝒂 ⋅ 𝒃 × 𝒄 = 𝒃 ⋅ 𝒄 × 𝒂 = 𝒄 ⋅ 𝒂 × 𝒃となることがわかる.(外積の順序に注意)
𝒂 ⋅ 𝒃 × 𝒄 = 0の場合逆行列は存在しない.
39
電気通信大学大学院情報理工学研究科情報学専攻 ロボットソフトウェア特論2016年度前学期
- 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())
電気通信大学大学院情報理工学研究科情報学専攻 ロボットソフトウェア特論2016年度前学期
- 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);
電気通信大学大学院情報理工学研究科情報学専攻 ロボットソフトウェア特論2016年度前学期
- MATRIXの回転角、回転軸
42
else :angl = 0.0axis[0] = 1.0axis[1] = 0.0axis[2] = 0.0
return([angl,axis])
電気通信大学大学院情報理工学研究科情報学専攻 ロボットソフトウェア特論2016年度前学期
- 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])
電気通信大学大学院情報理工学研究科情報学専攻 ロボットソフトウェア特論2016年度前学期
- ベクトルの生成例
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]]
電気通信大学大学院情報理工学研究科情報学専攻 ロボットソフトウェア特論2016年度前学期
- ベクトル化の使用例
45
>>> a.row(1)v:[0.0, 0.5000000000000001, -0.8660254037844386]>>> a.col(1)v:[0.0, 0.5000000000000001, 0.8660254037844386]
電気通信大学大学院情報理工学研究科情報学専攻 ロボットソフトウェア特論2016年度前学期
- 積,逆行列,転置の使用例(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]]
電気通信大学大学院情報理工学研究科情報学専攻 ロボットソフトウェア特論2016年度前学期
- 積,逆行列,転置の使用例(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]
電気通信大学大学院情報理工学研究科情報学専攻 ロボットソフトウェア特論2016年度前学期
- 回転角,回転軸,𝛼, 𝛽, 𝛾の使用例
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]
電気通信大学大学院情報理工学研究科情報学専攻 ロボットソフトウェア特論2016年度前学期
- 例題5-3
a=VECTOR(1,1,0)をx軸まわりに90度回転させたベクトルを求めよ。
ヒント:x軸周りに90度回転の回転行列を作り,aに左からかける.
49
電気通信大学大学院情報理工学研究科情報学専攻 ロボットソフトウェア特論2016年度前学期
- 例題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]
電気通信大学大学院情報理工学研究科情報学専攻 ロボットソフトウェア特論2016年度前学期
- 例題5-4
a=VECTOR(1,1,0)を回転させ、b=VECTOR(1,0,1)に一致させるための、x軸以外の回転軸を1つとそのまわりの回転角度をもとめよ。
ヒント: まずは回転軸を求める.どこにあるか.直観でも良い.
回転軸周りの回転角度は,例題5-2を参考にする.
51
電気通信大学大学院情報理工学研究科情報学専攻 ロボットソフトウェア特論2016年度前学期
- 例題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
電気通信大学大学院情報理工学研究科情報学専攻 ロボットソフトウェア特論2016年度前学期
- 例題5-5
例題5-4で求めた回転軸、回転角度でa=VECTOR(1,1,0)を回転させ、 b=VECTOR(1,0,1)に一致することを確認せよ。
53
電気通信大学大学院情報理工学研究科情報学専攻 ロボットソフトウェア特論2016年度前学期
- 例題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]
電気通信大学大学院情報理工学研究科情報学専攻 ロボットソフトウェア特論2016年度前学期
次回予告
座標系表現(FRAME)
python-visual (Vpython)
Vpythonをインストールしておくこと
55