読者です 読者をやめる 読者になる 読者になる

roombaの日記

読書・非線形科学・プログラミング・アート・etc...

テオ・ヤンセン機構の計算【詳細版】

はじめに

前回の記事ではテオ・ヤンセン機構のアニメーションを行いました。その際のリンク機構の詳細な動きに関する説明は省略していたのですが、思いのほかブックマークが沢山ついたのでここで説明してみます。


テオ・ヤンセン機構をHTML5 Canvasでアニメーションに - roombaの日記

結構面倒ですが、やっていることは点A~点Gの座標を計算しているだけです(前回の記事中プログラムのdraw_jansen関数に対応)。それさえできてしまえば、あとはその点どうしを線で結ぶだけでリンク機構が描けることになります。そこで、点A~点Gの座標の計算方法を以下で順に説明します。

前提知識:三角関数余弦定理


前提

  • 以下の図では、青字が点の名前・黒字がリンクの長さ
  • で示したリンクmが点Oのまわりを回転し、その角度Θによって他の全ての点の座標が定まる(1自由度)
  • 座標系の原点は点Oとし、右がx, 上がy
  • 点O, 点Bは固定点で不動
  • 点Aの座標を(A0, A1)のように表す
  • リンクの長さa~mは既知(前回の記事中プログラムで宣言)

1. 点Aの座標の計算

点Oが原点で、長さmのリンクが角度θの位置にあるため
{\displaystyle
\begin{eqnarray}
  \begin{cases}
    A_0 = m \;cos(\theta) & \\
    A_1 = m \;sin(\theta) &
  \end{cases}
\end{eqnarray}
}
これによってリンクm(線分OA)が描画できる。

2. 点Cの座標の計算

f:id:roomba:20150226012349j:plain
(i) 点Aと点Bの距離の計算
先ほどの点Aの座標と、点Bの座標が(-a, -l)であることより
{\displaystyle
AB = \sqrt{(a + A_0)^2 + (l + A_1)^2}
}

(ii) xc, ycの計算
図の緑字のようにxc, ycを定めると、余弦定理より
{\displaystyle
\begin{eqnarray}
  \begin{cases}
    x_c = b\;cos(\angle CBA) = \frac{AB^2 + b^2 - j^2}{2AB} & \\
    y_c = \sqrt{b^2 - x_c^2} &
  \end{cases}
\end{eqnarray}
}

(iii) theta_abの計算
図の緑のように角度theta_abを定めると
{\displaystyle
theta\_ab = arctan(\frac{A_1-B_1}{A_0-B_0})
}

(iv) 点Cの座標の計算
上記で求めたxc, yc, theta_abを用いて
{\displaystyle
\begin{eqnarray}
  \begin{cases}
    C_0 =  B_0 + x_c\;cos(theta\_ab) + y_c\;cos(theta\_ab + \frac{\pi}{2}) & \\
    C_1 =  B_1 + x_c\;sin(theta\_ab) + y_c\;sin(theta\_ab + \frac{\pi}{2}) &
  \end{cases}
\end{eqnarray}
}
上式はベクトルの足し算であり、第一項までが点Bの座標・第二項までが線分AB上の垂線の足の座標となっている。

これによってリンクj, b(線分AC, BC)が描画できる。

3. 点Eの座標の計算

f:id:roomba:20150226012350j:plain
(i) xc, ycの計算
今度はxc, ycを図のように定めると
{\displaystyle
\begin{eqnarray}
  \begin{cases}
    x_c = d\;cos(\angle CBE) = \frac{b^2 + d^2 - e^2}{2b} & \\
    y_c = \sqrt{d^2 - x_c^2} &
  \end{cases}
\end{eqnarray}
}

(ii) theta_bcの計算
肌色のようにtheta_bcを定めると
{\displaystyle
theta\_bc = arctan(\frac{C_1-B_1}{C_0-B_0})
}

(iii) 点Eの座標の計算

{\displaystyle
\begin{eqnarray}
  \begin{cases}
    E_0 =  B_0 + x_c\;cos(theta\_bc) + y_c\;cos(theta\_bc + \frac{\pi}{2}) & \\
    E_1 =  B_1 + x_c\;sin(theta\_bc) + y_c\;sin(theta\_bc + \frac{\pi}{2}) &
  \end{cases}
\end{eqnarray}
}
第一項までが点Bの座標・第二項までが線分BC上の垂線の足の座標となっている。

これによってリンクe, d(線分CE, BE)が描画できる。

4. 点Dの座標の計算

f:id:roomba:20150226015531j:plain
(i) xc, ycの計算
今度はxc, ycを図のように定めると
{\displaystyle
\begin{eqnarray}
  \begin{cases}
    x_c = c\;cos(\angle ABD) = \frac{AB^2 + c^2 - k^2}{2AB} & \\
    y_c = \sqrt{c^2 - x_c^2} &
  \end{cases}
\end{eqnarray}
}
(ii) 点Dの座標の計算
上記のxc, ycおよび既に求めていたtheta_abを用いると
{\displaystyle
\begin{eqnarray}
  \begin{cases}
    D_0 =  B_0 + x_c\;cos(theta\_ab) + y_c\;cos(theta\_ab -  \frac{\pi}{2}) & \\
    D_1 =  B_1 + x_c\;sin(theta\_ab) + y_c\;sin(theta\_ab - \frac{\pi}{2}) &
  \end{cases}
\end{eqnarray}
}
第一項までが点Bの座標・第二項までが線分AB上の垂線の足の座標となっている。

これによってリンクk, c(線分AD, BD)が描画できる。

5. 点Fの座標の計算

f:id:roomba:20150226015529j:plain
(i)点Dと点Eの距離の計算
既に求めた点D, Eの座標より
{\displaystyle
DE = \sqrt{(D_0 - E_0)^2 + (D_1 - E_1)^2}
}
(ii) xc, ycの計算
上記のDEを用いて、再びxc, ycを図のように定めると
{\displaystyle
\begin{eqnarray}
  \begin{cases}
    x_c = g\;cos(\angle EDF) = \frac{DE^2 + g^2 - f^2}{2DE} & \\
    y_c = \sqrt{g^2 - x_c^2} &
  \end{cases}
\end{eqnarray}
}
(iii) theta_deの計算
緑色のようにtheta_deを定めると
{\displaystyle
theta\_de = arctan(\frac{E_1-D_1}{E_0-D_0})
}
(iv) 点Fの座標の計算
上記のxc, yc, theta_deを用いると
{\displaystyle
\begin{eqnarray}
  \begin{cases}
    F_0 =  D_0 + x_c\;cos(theta\_de) + y_c\;cos(theta\_de +  \frac{\pi}{2}) & \\
    F_1 =  D_1 + x_c\;sin(theta\_de) + y_c\;sin(theta\_de +  \frac{\pi}{2}) &
  \end{cases}
\end{eqnarray}
}
第一項までが点Dの座標・第二項までが線分DE上の垂線の足の座標となっている。

これによってリンクf, g(線分EF, DF)が描画できる。

6. 点Gの座標の計算

f:id:roomba:20150226015530j:plain
(i) xc, ycの計算
今度はxc, ycを図のように定めると
{\displaystyle
\begin{eqnarray}
  \begin{cases}
    x_c = i\;cos(\angle GDF) = \frac{g^2 + i^2 - h^2}{2g} & \\
    y_c = \sqrt{i^2 - x_c^2} &
  \end{cases}
\end{eqnarray}
}
図ではxcが負になっているが、(iii)でちゃんと辻褄があう。
(ii) theta_dfの計算
肌色のようにtheta_dfを定めると
{\displaystyle
theta\_df = arctan(\frac{F_1-D_1}{F_0-D_0})
}
(iii) 点Gの座標の計算
{\displaystyle
\begin{eqnarray}
  \begin{cases}
    G_0 =  D_0 + x_c\;cos(theta\_df) + y_c\;cos(theta\_df +  \frac{\pi}{2}) & \\
    G_1 =  D_1 + x_c\;sin(theta\_df) + y_c\;sin(theta\_df +  \frac{\pi}{2}) &
  \end{cases}
\end{eqnarray}
}
第一項までが点Dの座標・第二項までが直線FD上の垂線の足の座標となっている。

これによってリンクh, i(線分FG, DG)が描画できる。

おわりに

疲れた…
このように、リンクmの角度θに依存した形で各点A~Gの座標を求めることができました。そのθを少しずつ増加させながら描画してゆけばアニメーションとなります。やったね!

ちなみに、θだけですべての座標が定まったのは、この機構が「1自由度」だからです。例えば肩と肘を持つロボットアームだと2自由度となり、肩と肘の角度をそれぞれ指定しなければなりません。

アニメーションは前回の記事↓