ペンを持った右腕の関節の角度計算 - 機械学習基礎理論独習

はじめに

自分の頭を整理するために本記事を書きます。
自分のためであり、本記事は他人に読まれることを想定していません。予めご了承ください。
回転行列 ${\bf R}_{\rm x}(\cdot),{\bf R}_{\rm y}(\cdot), {\bf R}_{\rm z}(\cdot)$ が 4x4 でも 3x3 でも同じ表記使われていますが、適宜読み替えてください

手のモデル

手のモデルは以下であるとします。
upperarm, forearm, hand が繋がっていませんが、繋がっていると考えてください。
あと、ねじれ解消用のボーンとindex(人差し指)以外の指のボーンはあえて非表示にしてあります。

肘と手首の位置

ペン先のワールド座標は、以下のようになります。

$\begin{eqnarray} {\bf M}_{\rm w}{\bf M}_{\rm c}{\bf M}_{\rm u}{\bf M}_{\rm f}{\bf M}_{\rm h}{\bf M}_{\rm i1}{\bf M}_{\rm i2}{\bf M}_{\rm i3}{\bf M}_{\rm p}\begin{pmatrix}{\bf 0}\\1\end{pmatrix}\tag{1} \end{eqnarray}$

${\bf M}_{\rm w}$ は、腕のワールド行列です。
${\bf M}_{\rm c},{\bf M}_{\rm u},{\bf M}_{\rm f},{\bf M}_{\rm h},{\bf M}_{\rm i1},{\bf M}_{\rm i2},{\bf M}_{\rm i3},{\bf M}_{\rm p}$
はそれぞれ clavicle, upperarm, forearm, hand, index1, index2, index3, pen のローカル変換行列です。

ペン先を決定するパラメータは、upperarmZ, upperarmX, upperarmY, forearmX, forearmY, handZ, handX, pen の 8つです。
upperarm,forearmのX,Y,Z軸回りの回転角度を $\theta_{\rm x},\theta_{\rm y},\theta_{\rm z},\phi_{\rm x},\phi_{\rm y},\phi_{\rm z}$ とします。
実際に腕を動かしてみれば分かりますが、上腕(upperarm)の自由度は3で、前腕(forearm)の自由度は2です。
$\phi_z$ は常に0で固定します。 $\phi_{\rm y}$ は肘と手首の位置には無関係です。
よって、肘と手首の位置を決定するパラメータは、 $\theta_{\rm z},\theta_{\rm x},\theta_{\rm y},\phi_{\rm x}$ の4つであることが分かります。
肘の位置は、 $\theta_{\rm z},\theta_{\rm x}$ によって決まり、肘から見た手首の位置は $\theta_{\rm y},\phi_{\rm x}$ によって決まります。

肘と手首の位置を決定する行列は ${\bf M}_{\rm u}{\bf M}_{\rm f}$ なので、ちょっと書き出してみます。

$\begin{eqnarray} {\bf M}_{\rm u}{\bf M}_{\rm f}&=&{\bf T}_{\rm u}{\bf R}_{\rm u}{\bf R}_{\rm z}(\theta_{\rm z}){\bf R}_{\rm x}(\theta_{\rm x}){\bf R}_{\rm y}(\theta_{\rm y}){\bf S}_{\rm u}{\bf T}_{\rm f}{\bf R}_{\rm f}{\bf R}_{\rm z}(\underbrace{\phi_{\rm z}}_{=0}){\bf R}_{\rm x}(\phi_{\rm x}){\bf R}_{\rm y}(\phi_{\rm _y}){\bf S}_{\rm f}\\ &=&{\bf T}_{\rm u}{\bf R}_{\rm u}{\bf R}_{\rm z}(\theta_{\rm z}){\bf R}_{\rm x}(\theta_{\rm x}){\bf R}_{\rm y}(\theta_{\rm y}){\bf S}_{\rm u}{\bf T}_{\rm f}{\bf R}_{\rm f}{\bf R}_{\rm z}(0){\bf R}_{\rm x}(\phi_{\rm x}){\bf R}_{\rm y}(\phi_{\rm _y}){\bf S}_{\rm f}\\ &=&{\bf T}_{\rm u}{\bf R}_{\rm u}{\bf R}_{\rm z}(\theta_{\rm z}){\bf R}_{\rm x}(\theta_{\rm x}){\bf R}_{\rm y}(\theta_{\rm y}){\bf S}_{\rm u}{\bf T}_{\rm f}{\bf R}_{\rm f}{\bf R}_{\rm x}(\phi_{\rm x}){\bf R}_{\rm y}(\phi_{\rm _y}){\bf S}_{\rm f}\tag{2} \end{eqnarray}$

モデルをいじる

腕のモデルは glTF2.0 で定義されています。
upperarm, forearm, hand の　glTFを覗いてみます。

forearm, hand の translation はほぼY軸に平行であり、rotation はほぼ単位行列であり、scale はほぼ単位行列であることが分かります。
forearm, hand の translation はY軸に平行、rotation は単位行列、scale は単位行列であるとして、式 $(2)$ を書き直します。

$\begin{eqnarray} {\bf M}_{\rm u}{\bf M}_{\rm f} &=&{\bf T}_{\rm u}{\bf R}_{\rm z}(\theta_{\rm z}){\bf R}_{\rm x}(\theta_{\rm x}){\bf R}_{\rm y}(\theta_{\rm y}){\bf T}_{\rm f}{\bf R}_{\rm x}(\phi_{\rm x}){\bf R}_{\rm y}(\phi_{\rm y})\tag{3} \end{eqnarray}$

translation をY軸に平行とみなす理由ですが、後で回転角度を計算するときに、回転角度が0の時の軸を $(0,1,0)^\top$ とするためです。
rotation を単位行列とみなす理由ですが、単位行列でないと ${\bf R}_{\rm y}(\theta_{\rm y}){\bf R}_{\rm f}{\bf R}_{\rm x}(\phi_{\rm x})$ のように ${\bf R}_{\rm y}(\theta_{\rm y}),{\bf R}_{\rm x}(\phi_{\rm x})$ の間に回転行列 ${\bf R}_{\rm f}$ が挟まり、 $\theta_{\rm y},\phi_{\rm x}$ を求めるのが困難になるためです。
scale を単位行列とみなす理由ですが、腕のような構造のモデルでは、拡大行列は使用しないのが普通ですし、成分によって拡大率が異なるといろいろな不都合が出るためです。
※upperarm, forearm のねじれ回避用のボーンに関しても同様の処理をしたほうが良いと思います。

式 $(3)$ は 4x4 ですが、回転成分だけに着目して 3x3 を考えてみます。

$\begin{eqnarray} {\bf R}_{\rm z}(\theta_{\rm z}){\bf R}_{\rm x}(\theta_{\rm x}){\bf R}_{\rm y}(\theta_{\rm y}){\bf R}_{\rm x}(\phi_{\rm x})\tag{4} \end{eqnarray}$

式 $(4)$ で ${\bf R}_{\rm y}(\phi_{\rm y})$ は手首の位置に無関係なので省略しました。

肘の位置から肩の角度を求める

さて、FABRIK などの IK で肘と手首のワールド座標が求まったとします。
前述したとおり、肘の位置は、 $\theta_{\rm z},\theta_{\rm x}$ によって決まるのでこれらを求めてみます。

upperarm の原点のワールド座標 ${\bf u}_{\rm w}$ は以下のように求まります。

$\begin{eqnarray} \begin{pmatrix}{\bf u}_{\rm w}\\ 1\end{pmatrix}={\bf M}_{\rm w}{\bf M}_{\rm c}{\bf T}_{\rm u}\begin{pmatrix}{\bf 0}\\1\end{pmatrix}\tag{5} \end{eqnarray}$

肘のワールド座標を ${\bf f}_{\rm w}$ とします。
${\bf M}_{\rm wc3}\in{\mathbb R}^{3\times 3}$ は ${\bf M}_{\rm w}{\bf M}_{\rm c}$ の平行移動成分を無くした 3x3 の行列とします。
このとき、upperarm の座標系における肘の位置までの方向ベクトル ${\bf a}\in{\mathbb R}^3$ は、以下のように求まります。

$\begin{eqnarray} {\bf a}={\bf M}_{\rm wc3}^{-1}\left({\bf f}_{\rm w}-{\bf u}_{\rm w}\right)\tag{6} \end{eqnarray}$

${\bf a}$ を単位ベクトル化します。

$\begin{eqnarray} {\bf a}\leftarrow\frac{1}{||{\bf a}||}{\bf a}\tag{7} \end{eqnarray}$

$\theta_{\rm z} = 0, \theta_{\rm x}=0$ のときの upperarm の座標系における肘の位置までの方向ベクトルは ${\bf e}_{\rm y}=(0,1,0)^\top$ なので計算不要です。
${\bf e}_{\rm y}$ を ${\bf a}$ に一致させるような $\theta_{\rm z}, \theta_{\rm x}$ を求めます。
まず、 ${\bf e}_{\rm y}$ を ${\bf a}$ に一致させるような回転軸 ${\bf r}_a\in{\mathbb R}^3$ と回転角度 $\theta_a\in{\mathbb R}$ を求めます。

$\begin{eqnarray} &&{\bf r}_a={\bf e}_{\rm y}\times{\bf a}\tag{8}\\ &&\theta_a=\cos^{-1}({\bf e}_{\rm y}^\top{\bf a})\tag{9} \end{eqnarray}$

回転軸 ${\bf r}_a$ を単位ベクトル化しておきます。

$\begin{eqnarray} {\bf r}_a\leftarrow\frac{1}{||{\bf r}_a||}{\bf r}_a\tag{10} \end{eqnarray}$

次に、回転軸 ${\bf r}_a$ と回転角度 $\theta_a$ から回転行列 ${\bf M}_a\in{\mathbb R}^{3\times 3}$ を求めます。
回転行列の導出については、任意軸まわりの回転 - ロドリゲスの回転公式を参照してください。

最後に、回転行列 ${\bf M}_a$ から $\theta_{\rm z}, \theta_{\rm x}$ を求めます。

$\begin{eqnarray} {\bf R}_{\rm z}(\theta_{\rm z}){\bf R}_{\rm x}(\theta_{\rm x}){\bf R}_{\rm y}(\theta_{\rm y})= \begin{pmatrix} -\sin{\theta_{\rm x}} \sin{\theta_{\rm y}} \sin{\theta_{\rm z}} + \cos{\theta_{\rm y}} \cos{\theta_{\rm z}} & - \sin{\theta_{\rm z}} \cos{\theta_{\rm x}} & \sin{\theta_{\rm x}} \sin{\theta_{\rm z}} \cos{\theta_{\rm y}} + \sin{\theta_{\rm y}} \cos{\theta_{\rm z}}\\\sin{\theta_{\rm x}} \sin{\theta_{\rm y}} \cos{\theta_{\rm z}} + \sin{\theta_{\rm z}} \cos{\theta_{\rm y}} & \cos{\theta_{\rm x}} \cos{\theta_{\rm z}} & - \sin{\theta_{\rm x}} \cos{\theta_{\rm y}} \cos{\theta_{\rm z}} + \sin{\theta_{\rm y}} \sin{\theta_{\rm z}}\\- \sin{\theta_{\rm y}} \cos{\theta_{\rm x}} & \sin{\theta_{\rm x}} & \cos{\theta_{\rm x}} \cos{\theta_{\rm y}} \end{pmatrix}\tag{11} \end{eqnarray}$

成分を比較すると、以下のように求まります。

$\begin{eqnarray} &&\theta_{\rm z}={\rm atan2}(-{\bf M}_{a01}, {\bf M}_{a11})\tag{12}\\ &&\theta_{\rm x}=\sin^{-1}({\bf M}_{a21})\tag{13} \end{eqnarray}$

$\cos{\theta_{\rm x}}=0$ のときにジンバルロックがかかりますが、その時 ${\rm atan2}(0,0)$ は $0$ を返すので、そのままでよいと思います。
私の作成しているアプリの場合、ジンバルロックを気にするような角度になることはありません。

手首の位置から肩と肘の角度を求める

手首のワールド座標を ${\bf h}_{\rm w}$ とします。
${\bf M}_{\rm wcu3}\in{\mathbb R}^{3\times 3}$ は ${\bf M}_{\rm w}{\bf M}_{\rm c}{\bf R}_{\rm z}(\theta_{\rm z}){\bf R}_{\rm z}(\theta_{\rm x})$ の平行移動成分を無くした 3x3 の行列とします。
このとき、forearm の座標系における肘の位置までの方向ベクトル ${\bf b}\in{\mathbb R}^3$ は、以下のように求まります。

$\begin{eqnarray} {\bf b}={\bf M}_{\rm wcu3}^{-1}\left({\bf h}_{\rm w}-{\bf f}_{\rm w}\right)\tag{14} \end{eqnarray}$

$\theta_{\rm z} = 0, \theta_{\rm x}=0$ のときの forearm の座標系における手首の位置までの方向ベクトルは ${\bf e}_{\rm y}=(0,1,0)^\top$ なので計算不要です。
${\bf e}_{\rm y}$ を ${\bf b}$ に一致させるような $\theta_{\rm y}, \phi_{\rm x}$ を求めます。
まず、 ${\bf e}_{\rm y}$ を ${\bf b}$ に一致させるような回転軸 ${\bf r}_b\in{\mathbb R}^3$ と回転角度 $\theta_b\in{\mathbb R}$ を求めます。

$\begin{eqnarray} &&{\bf r}_b={\bf e}_{\rm y}\times{\bf b}\tag{15}\\ &&\theta_b=\cos^{-1}({\bf e}_{\rm y}^\top{\bf b})\tag{16} \end{eqnarray}$

回転軸 ${\bf r}_b$ を単位ベクトル化しておきます。

$\begin{eqnarray} {\bf r}_b\leftarrow\frac{1}{||{\bf r}_b||}{\bf r}_b\tag{17} \end{eqnarray}$

次に、回転軸 ${\bf r}_b$ と回転角度 $\theta_b$ から回転行列 ${\bf M}_b\in{\mathbb R}^{3\times 3}$ を求めます。
回転行列の導出については、任意軸まわりの回転 - ロドリゲスの回転公式を参照してください。

最後に、回転行列 ${\bf M}_b$ から $\theta_{\rm y}, \phi_{\rm x}$ を求めます。

$\begin{eqnarray} {\bf R}_{\rm y}(\theta_{\rm y}){\bf R}_{\rm x}(\phi_{\rm x}){\bf R}_{\rm y}(\phi_{\rm y})= \begin{pmatrix} - \sin{\phi_{\rm y}} \sin{\theta_{\rm y}} \cos{\phi_{\rm x}} + \cos{\phi_{\rm y}} \cos{\theta_{\rm y}} & \sin{\phi_{\rm x}} \sin{\theta_{\rm y}} & \sin{\phi_{\rm y}} \cos{\theta_{\rm y}} + \sin{\theta_{\rm y}} \cos{\phi_{\rm x}} \cos{\phi_{\rm y}}\\ \sin{\phi_{\rm x}} \sin{\phi_{\rm y}} & \cos{\phi_{\rm x}} & - \sin{\phi_{\rm x}} \cos{\phi_{\rm y}}\\ - \sin{\phi_{\rm y}} \cos{\phi_{\rm x}} \cos{\theta_{\rm y}} - \sin{\theta_{\rm y}} \cos{\phi_{\rm y}} & \sin{\phi_{\rm x}} \cos{\theta_{\rm y}} & - \sin{\phi_{\rm y}} \sin{\theta_{\rm y}} + \cos{\phi_{\rm x}} \cos{\phi_{\rm y}} \cos{\theta_{\rm y}} \end{pmatrix}\tag{18} \end{eqnarray}$

成分を比較すると、以下のように求まります。

$\begin{eqnarray} &&\theta_{\rm y}={\rm atan2}(-{\bf M}_{a01}, {\bf M}_{a21})\tag{19}\\ &&\phi_{\rm x}=\cos^{-1}({\bf M}_{a11})\tag{20} \end{eqnarray}$

$\sin{\phi_{\rm x}}=0$ のとき(肘を伸ばしている)にジンバルロックがかかりますが、その時 ${\rm atan2}(0,0)$ は $0$ を返すので、そのままでよいと思います。
私の作成しているアプリの場合、ジンバルロックを気にするような角度になることはありません。

参考リンク

回転行列、クォータニオン(四元数)、オイラー角の相互変換

参考文献

コンピュータグラフィックスの基礎 p39-p40