ロボットソフトウェア特論 (3-2)suehiro/class2017/presen/lecture03-2.pdf · - VECTORのクラス定義 listのサブクラスとして定義 8 class VECTOR(list) : def init(self,

ロボットソフトウェア特論 (3-2)

2017.4.26

電気通信大学

大学院情報理工学研究科

末廣尚士

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

4. Pythonによる幾何演算プログラム

幾何演算ベクトル：スカラー積、内積、外積、行列との積

行列：積、行列との積

座標変換：座標系、ベクトルとの積（座標変換）

手軽に使える自分用のツール理解のためのプログラム

精度、スピードはある意味度外視

そういうことが必要な場合は、他のライブラリを使う

2


- 幾何演算モジュール

VECTOR, MATRIX, FRAMEの3つの幾何演算用のクラスをサポートする．

オブジェクト指向

VECTOR: 3次元ベクトル，３次元位置ベクトル

MATRIX: 3次元回転行列

FRAME: 3次元座標，3次元座標変換

3


- 幾何演算モジュール: geo.py使い方あらかじめパスを通しておく

またはcurrent directoryに置く

math モジュールも読み込まれることに注意

4

$ python...>>> from geo import *


- ベクトル

座標系：Σ0

ベクトル： 0𝒗 =

𝑣x𝑣y𝑣z

位置ベクトル: 0𝒑 =

𝑝x𝑝y𝑝z

5

Σ0

z

y

x

0𝒗

0𝒑


- ベクトルと位置ベクトル

見る座標系を変えた場合，

ベクトル：表現は違うが同じベクトル．

位置ベクトル：表す位置は同じ．ベクトルとしては表現が違うだけでなく，違うベクトル．

6

Σ0

z

y

x

0𝒗

0𝒑

Σ1

z

y

x

1𝒑

1𝒗


- 三次元ベクトルのプログラム

ベクトルの構造生成方法

成分の値の参照，変更

ベクトルの表示

ベクトルの演算（関数，メソッド）和，差

符号反転

内積，外積

絶対値，（正規化）

スカラー倍

行列との積

7


- VECTORのクラス定義listのサブクラスとして定義

8

class VECTOR(list) :def __init__(self, x=0.0, y=0.0, z=0.0, vec=[]) :

クラス名．ユーザが作るクラスは先頭だけ大文字にすることも多い

list のサブクラスとして定義引数ではない

生成，初期化関数

引数の先頭はインスタンスを指す．Pythonのオブジェクト関数では必須の引数．一般の呼び出しで対応する引数はシステムが自動的に割り当てる．変数名はselfでなくても良いが，，，

残りの引数は処理に応じて好きなようにして良い．


- VECTORのクラス定義

9

class VECTOR(list) :def __init__(self, x=0.0, y=0.0, z=0.0, vec=[]) :list.__init__(self, [float(x),float(y),float(z)])if vec :

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

親クラスの初期化ルーチンを呼び出す．明示的に，親のクラスを指定しているので呼びだせる．この呼び方では引数にselfが必要となる．

段下げ注意

先頭がアンダースコア2つの関数や変数は明示的に指定しない限り外部のオブジェクトから呼び出せない．アンダースコア2つで囲まれた関数（__init__など）や変数はシステムが使うことを想定している．


- VECTORの表示

10

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

親クラスの表示ルーチンを呼び出す．

段下げ注意

自分自身を指す引数これもシステムが呼び出す関数


x, y, zを与えて生成

省略も可能．省略されると 0.0 になる

- VECTORの生成（１）

11

>>> a=VECTOR(1,2,3)>>> av:[1.0, 2.0, 3.0]

自動的に表示関数が呼び出されている．呼び出しを意識する必要もない．

>>> VECTOR()v:[0.0, 0.0, 0.0]

自動的に初期化関数が呼び出されている．selfに対応する引数は不要


ｘ, y, z をキーワードで指定して生成

リスト，タプル，VECTORのコピーによる生成

- VECTORの生成（２）

12

>>> b=VECTOR(y=3)>>> bv:[0.0, 3.0, 0.0]

>>> VECTOR(vec=a)v:[1.0, 2.0, 3.0]


- ベクトルの内積の復習

𝒗1 =

𝑣1,x𝑣1,y𝑣1,z

𝒗2 =


𝒗1 ∙ 𝒗2 = 𝒗1 𝒗2 cos 𝜃= 𝑣1,x𝑣2,x + 𝑣1,y𝑣2,y + 𝑣1,z𝑣2,z

13


内積：外から呼び出される関数

- VECTORの内積（コード）

14

def dot(self, other) :return(self[0]*other[0] +

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

段下げ注意

自分自身演算の相手普通の関数名


内積：a.dot(b)

- VECTORの内積（使用例）

15

>>> av:[1.0, 2.0, 3.0]>>> bv:[0.0, 3.0, 0.0]>>> a.dot(b)6.0>>>

呼び出すときの引数には自分自身(self)は書かなくてよい.システムが自動的に入れる


和：self + other

- VECTORの和

16

def __add__(self, other) :tmp = VECTOR();for i in [0,1,2] :

tmp[i] = self[i] + other[i]return(tmp)

これもシステム関数「+」による加算のときに自動的に呼び出される

演算の相手

この__add__関数は自分が「+」の左にあるときに呼び出される．


和：a + b

- VECTORの和

17

>>> av:[1.0, 2.0, 3.0]>>> bv:[0.0, 3.0, 0.0]>>> a+bv:[1.0, 5.0, 3.0]>>>


和：a + [1,2,3], [1,2,3]+a

これはまだよいとして

これは大問題

- VECTORの和の問題点

18

>>> av:[1.0, 2.0, 3.0]>>> a+[1,2,3]v:[2.0, 4.0, 6.0]

>>> [1,2,3]+a[1, 2, 3, 1.0, 2.0, 3.0]


和：other + self

- VECTORのもう一つの和

19

def __radd__(self, other) :tmp = VECTOR();for i in [0,1,2] :

tmp[i] = other[i] + self[i]return(tmp)

この__radd__関数は自分が「+」の右(right)にあるときに呼び出される．

演算の相手.引数の順番と演算の順序は関係ない．

演算子に対応する関数の検索は，演算子の左側のオブジェクトが優先．したがって「+」左に「自分を__add__できる」オブジェクトが来ると，この__radd__は使われない．


和： [1,2,3]+a

このようなlistとの加算も許したくないなら，引数の型チェックを行い「例外（エラー）」を出す必要がある．

- VECTORの __radd__

20

>>> av:[1.0, 2.0, 3.0]>>> [1,2,3]+av:[2.0, 4.0, 6.0]


- 演算子用の関数どんなものがあるか

21

>>> dir(1)['__abs__', '__add__', '__and__', '__class__', '__cmp__', '__coerce__', '__delattr__', '__div__', '__divmod__', '__doc__', '__float__', '__floordiv__', '__format__', '__getattribute__', '__getnewargs__', '__hash__', '__hex__', '__index__', '__init__', '__int__', '__invert__', '__long__', '__lshift__', '__mod__', '__mul__', '__neg__', '__new__', '__nonzero__', '__oct__', '__or__', '__pos__', '__pow__', '__radd__', '__rand__', '__rdiv__', '__rdivmod__', '__reduce__', '__reduce_ex__', '__repr__', '__rfloordiv__', '__rlshift__', '__rmod__', '__rmul__', '__ror__', '__rpow__', '__rrshift__', '__rshift__', '__rsub__', '__rtruediv__', '__rxor__', '__setattr__', '__sizeof__', '__str__', '__sub__', '__subclasshook__', '__truediv__', '__trunc__', '__xor__', 'bit_length', 'conjugate', 'denominator', 'imag', 'numerator', 'real']


差： a - b,

- VECTORの差

22

def __sub__(self, other) :tmp = VECTOR();for i in [0,1,2] :tmp[i] = self[i] - other[i]

return(tmp)

>>> av:[1.0, 2.0, 3.0]>>> bv:[0.0, 3.0, 0.0]>>> a-bv:[1.0, -1.0, 3.0]>>>


差の__rsub__は必要か？

このように無事にエラーになることで良しとするなら，無駄に定義をする必要はないかな．．．(でも「＋」とは整合していない)

- VECTORの __rsub__

23

>>> a-[1,2,3]v:[0.0, 0.0, 0.0]>>> [1,2,3]-a

Traceback (most recent call last):File "<pyshell#19>", line 1, in <module>[1,2,3]-a

TypeError: unsupported operand type(s) for -: 'list' and 'VECTOR'


符号反転：- a

- VECTORの符号反転

24

def __neg__(self) :return(VECTOR(-self[0],-self[1],-self[2]))

>>> av:[1.0, 2.0, 3.0]>>> -av:[-1.0, -2.0, -3.0]>>>


乗算「*」で扱いたいのは，

スカラー倍

外積

行列との掛け算

（内積はdot関数として別途定義した）

演算対象の型（クラス）で区別して処理する

- VECTORの乗算(「*」)

25


- ベクトルの外積の復習

𝒗1 =


， 𝒗2 =


𝒗1 × 𝒗𝟐 =

𝑣1,y𝑣2,z − 𝑣1,z𝑣2,y𝑣1,z𝑣2,x − 𝑣1,x𝑣2,z𝑣1,x𝑣2,y − 𝑣1,y𝑣2,x

𝒗1 × 𝒗𝟐 = 𝒗1 𝒗2 sin 𝜃𝒗1 ∙ 𝒗1 × 𝒗𝟐 = 𝟎𝒗2 ∙ 𝒗1 × 𝒗𝟐 = 𝟎

26


- VECTORの乗算(「*」)いろいろ

27

def __mul__(self, other) :if not isinstance(other, VECTOR) :

raise TypeErrortmp = VECTOR();tmp[0] = self[1]*other[2] - self[2]*other[1]tmp[1] = self[2]*other[0] - self[0]*other[2]tmp[2] = self[0]*other[1] - self[1]*other[0]return(tmp)

def __rmul__(self, other) :tmp = VECTOR();if isinstance(other, VECTOR) :pass

elif isinstance(other, MATRIX) :pass

else :for i in [0,1,2] :tmp[i] = other * self[i]

return(tmp)

外積

__mul＿があるので不要

右がVECTORでない場合は例外を発生

行列との積の定義は行列の__mul__で

残りはスカラー倍

自分が右の場合


スカラー倍: 3*a

- ベクトルのスカラー倍

28

>>> a=VECTOR(1,2,3)V:[1.0, 2.0, 3.0]>>> 3*av:[3.0, 6.0, 9.0]>>> a*3

Traceback (most recent call last):File "<pyshell#42>", line 1, in <module>a*3

File "C:¥Users¥suehiro¥Dropbox¥lecture¥2014program¥geo.py", line 55, in __mul__

tmp[0] = self[1]*other[2] - self[2]*other[1]TypeError: 'int' object has no attribute '__getitem__'


外積: a*b

- ベクトルの外積

29

>>> a=VECTOR(1,2,3)>>> aV:[1.0, 2.0, 3.0]>>> b=VECTOR(0,3,0)>>> bv:[0.0, 3.0, 0.0]>>> a*bv:[-9.0, 0.0, 3.0]>>> a*[1,2,3]

Traceback (most recent call last):File "<pyshell#51>", line 1, in <module>a*[1,2,3]

File "C:¥Users¥suehiro¥Dropbox¥lecture¥2014program¥geo.py", line 55, in __mul__

raise TypeErrorTypeError


絶対値: |a|

- VECTORの絶対値

30

def __abs__(self) :return(sqrt(self.dot(self)))

>>> av:[1.0, 2.0, 3.0]>>> abs(a)3.7416573867739413>>> a.__abs__()3.7416573867739413


- 例題4-1

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

31


- 例題4-1の解答例

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

32

>>> 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)>>> th0.26377986630106198>>> th*180/pi15.113473059575982


- 例題4-2

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

33


- 例題4-2の失敗例


34

>>> x=VECTOR(1,0,0)>>> a=VECTOR(-0.707,-0.707,0)>>> co=a.dot(x)/abs(a)/abs(x)>>> th=acos(co)>>> th2.3561944901923448>>> th*180/pi135.0


- 例題4-2の解答例


35

>>> 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


- 例題4-2の補足

36

zy

x

𝒂 𝒂 sin 𝜃

𝒂 cos 𝜃

𝜃


次回予告

回転行列

37

Documents

ロボットソフトウェア特論 (3-2)suehiro/class2017/presen/lecture03-2.pdf · - VECTORのクラス定義 listのサブクラスとして定義 8 class VECTOR(list) : def __init__(self,

ロボットソフトウェア特論 (3-2)suehiro/class2017/presen/lecture03-2.pdf · - VECTORのクラス定義 listのサブクラスとして定義 8 class VECTOR(list) : def init(self,