座標系の説明

座標変換

3次元の位置

3次元の位置は小文字の太字を用いて表される、実数3つを要素とするベクトルである。

3つの要素はそれぞれ、x軸、y軸、z軸の軸上の位置を表す。

縦ベクトルとして表すことが多い。

3次元の位置 \(\mathbf{p}\) は以下のように定義されている。

\[\mathbf{p} = [ \; x, \; y, \; z \;]^{T}\]

3次元の回転

3次元の回転は、回転行列を用いて表すことができる。

回転行列は3x3の行列で、基準座標系から見た回転された座標系列の x軸、y軸、z軸を列方向に並べたものである。

回転行列 \(\mathbf{R}\) は以下のように定義されている。

\[\mathbf{R} = [ \; \mathbf{x} \quad \mathbf{y} \quad \mathbf{z} \; ]\]

この行列は直交行列となっており、以下のように、行列の転置と逆行列が同値となる。

回転行列の逆行列は以下のようになる。

\[\mathbf{R}^{-1} = \mathbf{R}^{T}\]

3次元の回転の表し方としては、他に、ロール・ピッチ・ヨー角(RPY)、単位クォータニオン(quaternion)、軸回り回転(AngleAxis)、等がある。

情報量としては3であるが、計算に使いやすい回転行列や、特異点がなく補間が容易なクォータニオンが使われることが多い。

参考書を参照いただきたい。

軸回り回転(AngleAxis)

同時変換行列

3次元での姿勢は、3次元位置と3次元回転を用いて表すことができ、位置の3自由度と回転の3自由度で6自由度となっている。

3次元での6自由度を表現するために同時変換行列を用いる。

3次元での姿勢を表すのに、同次変換行列 T を用いる。 T は 3次元位置ベクトル \(\mathbf{p}\) と 3次元回転行列 \(\mathbf{R}\) を用いて、以下のように4x4行列として表される。

\[\begin{split}T = \begin{pmatrix} \mathbf{R} & \mathbf{p} \\ \mathbf{0} & 1 \end{pmatrix}\end{split}\]

T の逆行列は以下のようになる。

\[\begin{split}T^{-1} = \begin{pmatrix} \mathbf{R}^{-1} & - \mathbf{R}^{-1}\mathbf{p} \\ \mathbf{0} & 1 \end{pmatrix}\end{split}\]

T の掛け算は、\(\mathbf{T}_{a}\) と \(\mathbf{T}_{b}\) を添え字として以下のようになる。

\[\begin{split}T_a \times T_b = \begin{pmatrix} \mathbf{R}_a\mathbf{R}_b & \mathbf{R}_a\mathbf{p}_b + \mathbf{p}_a \\ \mathbf{0} & 1 \end{pmatrix}\end{split}\]

\(\mathbf{T}_{a}\) と \(\mathbf{T}_{b}\) は、以下のように表される。

\[\begin{split}T_a = \begin{pmatrix} \mathbf{R}_a & \mathbf{p}_a \\ \mathbf{0} & 1 \end{pmatrix}\end{split}\]

\[\begin{split}T_b = \begin{pmatrix} \mathbf{R}_b & \mathbf{p}_b \\ \mathbf{0} & 1 \end{pmatrix}\end{split}\]

座標系

剛体リンクの座標系

座標系とcoordinatesクラスの関係

coordinatesクラス (cnoid.IRSLCoords.coordinates) は、同次変換行列の操作を行うためのクラスである。

coordinatesクラスのインスタンスは、 3次元位置ベクトル \(\mathbf{p}\) と 3次元回転行列 \(\mathbf{R}\) を持つ。

初期化と回転行列と３次元位置の取り出し

coordinates のプロパティとして、以下のように \(\mathbf{p}\) と \(\mathbf{R}\) を取り出せる。

以下、Tはcoordinateクラスのインスタンスである。

Tの数学的な表現は以下の通り。

\[\begin{split}T = \begin{pmatrix} \mathbf{R} & \mathbf{p} \\ \mathbf{0} & 1 \end{pmatrix}\end{split}\]

coordinatesクラスの初期化

>>> p = numpy.array([1, 2, 3])
>>> R = numpy.array([[0, -1, 0],[1, 0, 0], [0, 0, 1]])
>>> T = coordinates(v, R)
>>> T
<coordinates[address] 1 2 3 / 0 0 0.707107 0.707107 >
>>> coordinates(p) ### set pos, rot is Identity
>>> coordinates(R) ### set rot, pos is  Zero
>>> coordinates(numpy.array([0, 0, 0, 1])) ### set rot by quaternion
>>> coordinates(v, numpy.array([0, 0, 0, 1])) ### set pos and rot by quaternion
>>> coordinates(numpy.array([[0, -1, 0, 0],[1, 0, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]) ### 4x4 homogeneous transformation matrix

プロパティ

pos

3次元位置のゲットとセット(attribute pos へのアクセス)

>>> T.pos
array([1., 2., 3.])

rot

回転行列のゲットとセット(attribute rot へのアクセス)

>>> T.rot
array([[ 0., -1.,  0.],
       [ 1.,  0.,  0.],
       [ 0.,  0.,  1.]])

quaternion

クオータニオンのゲットとセット

>>> T.quaternion
array([0.        , 0.        , 0.70710678, 0.70710678])

RPY

RPY角度のゲットとセット

>>> T.RPY
array([ 0.        , -0.        ,  1.57079633])

angleAxis

軸回り回転(AngleAxis)のゲットとセット

>>> T.angleAxis
array([0.        , 0.        , 1.        , 1.57079633])

homogeneousTransformation

4x4同時変換行列のゲットとセット

>>> T.homogeneousTransformation
array([[ 0., -1.,  0.,  1.],
       [ 1.,  0.,  0.,  2.],
       [ 0.,  0.,  1.,  3.],
       [ 0.,  0.,  0.,  1.]])

変換したベクトルを返すメソッド(元のベクトルを変更しない)

以下、 \(\mathbf{v}\) は3次元ベクトル (numpy.array) である。以下の4つの関数は入力オブジェクトの値を変更しない。

rotate_vector(vec)

ベクトルを回転させる

>>> v = numpy.array([0.1, 0.2, 0.3])
>>> T.rotate_vector(v)
array([-0.2,  0.1,  0.3])

注釈

返り値の数学的表現は

\(\mathbf{R} \mathbf{v}\)

inverse_rotate_vector(vec)

ベクトルを回転させる（逆回転）

>>> T.inverse_rotate_vector(v)

注釈

返り値の数学的表現は

\(\mathbf{v}^T \mathbf{R}\)

transform_vector(vec)

ベクトルを座標系を使って変換する

ローカル座標系Tで表現されたベクトルのワールド座標系への変換

>>> T.transform_vector(v)

注釈

返り値の数学的表現は

\(\mathbf{R}\mathbf{v} + \mathbf{p}\)

inverse_transform_vector(vec)

ベクトルを座標系を使って変換する（逆変換）

ワールド座標系で表現されたベクトルのローカル座標系Tへの変換

>>> T.inverse_transform_vector(v)

注釈

返り値の数学的表現は

\(\mathbf{R}^{-1}\left( \mathbf{v} - \mathbf{p} \right)\)

ベクトルを座標系を使って変換するメソッド（元のベクトルを変更する）

これらの関数は入力オブジェクトの値を変更する。

入力オブジェクトの値は返り値と同じ値になる。

>>> v = numpy.array([0.1, 0.2, 0.3])
>>> T.rotateVector(v)
>>> T.inverseRotateVector(v)
>>> T.transformVector(v)
>>> T.inverseTransformVector(v)

座標系を返すメソッド(自身のインスタンスの値を変更しない)

以下で、A は、coordinatesクラスのインスタンスである。

inverse_transformation()

自身の逆変換を返す

>>> T.inverse_transformation()

注釈

返り値の数学的表現は以下の通り。

\[\begin{split}T^{-1} = \begin{pmatrix} \mathbf{R}^{-1} & - \mathbf{R}^{-1}\mathbf{p} \\ \mathbf{0} & 1 \end{pmatrix}\end{split}\]

transformation(target_coords, [ wrt ])

2つの座標間の変換を得る

>>> T.transformation(A, wrt)

注釈

wrt は座標系を表す引数である。デフォルト値は 'local' である。

wrt が coordinates.wrt.local の時は

\(T^{-1}A\) が返る。

wrt が coordinates.wrt.world の時は

\(AT^{-1}\) が返る。

wrt が W (coordinates class) の時は

\(W^{-1}AT^{-1}W\) が返る。

get_transformed(target_coords, [ wrt ])

自身のインスタンスの値を変更せずに座標変換した座標系を得る

>>> result = T.get_transform(A, wrt)

注釈

wrt は座標系を表す引数である。デフォルト値は 'local' である。

wrt が coordinates.wrt.local の時は

\(TA\)

wrt が coordinates.wrt.world の時は

\(AT\)

wrt が W (coordinates class) の時は

\(\left( W A W^{-1} \right) T\)

自身のインスタンスの値を変更するメソッド

以下で、 \(\leftarrow\) は代入を表す。

newcoords(target_coords)

自身に新しい座標系をセットする

>>> T.newcoords(A)

注釈

attributes pos と rot に値が代入される。

\(T \leftarrow A\)

move_to(target_coords, [ wrt ])

自身を対象の座標系へ動かす

>>> T.move_to(A, wrt)

注釈

wrt は座標系を表す引数である。デフォルト値は 'local' である。

wrt が coordinates.wrt.local の時は

\(T \leftarrow TA\)

wrt が coordinates.wrt.world の時は

\(T \leftarrow A\)

wrt が W (coordinates class) の時は

\(T \leftarrow WA\)

translate(vec, [ wrt ])

ベクトルを用いて平行移動する（自身を変更する）

>>> T.translate(v, wrt)

注釈

wrt は座標系を表す引数である。デフォルト値は 'local' である。

wrt が coordinates.wrt.local の時は

\(\mathbf{p} \leftarrow \mathbf{p} + \mathbf{R}\mathbf{v}\)

wrt が coordinates.wrt.world の時は

\(\mathbf{p} \leftarrow \mathbf{p}+ \mathbf{v}\)

wrt が W (coordinates class) の時は

\(\mathbf{p} \leftarrow \mathbf{p} + \mathbf{R}_{W}\mathbf{v}\)

\(\mathbf{R}_{W}\) は W の回転行列

locate(vec, [ wrt ])

指定された位置に自身を配置する

>>> T.locate(v, wrt)

注釈

wrt は座標系を表す引数である。デフォルト値は 'local' である。

wrt が coordinates.wrt.local の時は

\(\mathbf{p} \leftarrow \mathbf{p} + \mathbf{R} \mathbf{v}\)

wrt が coordinates.wrt.world の時は

\(\mathbf{p} \leftarrow \mathbf{v}\)

wrt が W (coordinates class) の時は

\(\mathbf{p} \leftarrow \mathbf{p}_{W} + \mathbf{R}_{W} \mathbf{v}\)

\(\mathbf{R}_{W}\) は Wの回転行列、\(\mathbf{p}_{W}\) は Wの3次元位置。

transform(target_coords, [ wrt ])

座標系を使って自身を変換する（自身の値を変更する）

>>> T.transform(A, wrt)

注釈

wrt は座標系を表す引数である。デフォルト値は 'local' である。

wrt が coordinates.wrt.local の時は

\(T \leftarrow TA\)

wrt が coordinates.wrt.world の時は

\(T \leftarrow AT\)

wrt が W (coordinates class) の時は

\(T \leftarrow \left( W A W^{-1} \right) T\)

Examples

https://github.com/IRSL-tut/CPS-lecture/blob/main/notebooks/cps_lecture_coords00.ipynb

https://github.com/IRSL-tut/CPS-lecture/blob/main/notebooks/cps_lecture_coords01.ipynb

https://github.com/IRSL-tut/CPS-lecture/blob/main/notebooks/cps_lecture_cascaded_coordinates.ipynb

https://github.com/IRSL-tut/CPS-lecture/blob/main/notebooks/cps_lecture_coords_with_docs.ipynb

参考文献

実践ロボット制御 https://www.ohmsha.co.jp/book/9784274224300/

第2章姿勢の記述及び第4章運動学の一般的表現の内容が参考になる