目標の位置から角度を求める
逆運動学（インバースキネマティクス）は、手先を置きたい位置から、関節の角度を求める計算です。ロボットアームに「この点へ行け」と命じるとき、必要なのは角度です。
答えは、2つあることも、1つのことも、まったくないこともあります。このレッスンでは、その場合分けを学びます。
このレッスンのゴール
メモ
2つの関節の腕について、目標に届くかを調べ、届くなら2つの関節の角度を求められる。
キーワード
| ことば | いみ |
|---|---|
| 逆運動学 | 手先の位置から関節角度を求めること |
| 目標 | 手先を置きたい点 |
| 届く範囲 | 手先が置ける点の集まり |
| 解 | 目標に届く関節角度の組 |
| 余弦定理 | 三角形の3辺と1つの角をつなぐ式 |
学生が入力するセルは、このプレビューでは空白です。
まず目標に届くかを調べる
目標までの距離を d とします。腕が届くのは、d が「l1 と l2 の差」以上で、「l1 と l2 の和」以下のときだけです。
遠すぎる目標も、近すぎる目標も、角度は見つかりません。先にこの検査をすると、無理な計算を避けられます。
例: l1 が 2、l2 が 1 の腕
| 目標 | 距離 d | 届くか | 解の数 |
|---|---|---|---|
| (2, 1) | 約 2.24 | 届く | 2つ |
| (3, 0) | 3 | 届く（まっすぐ） | 1つ |
| (4, 0) | 4 | 遠すぎる | なし |
| (0.5, 0) | 0.5 | 近すぎる | なし |
学生が入力するセルは、このプレビューでは空白です。
目標 (4, 0) は遠すぎるか
docker exec jupyter python3 -c "import numpy as np; print(np.hypot(4, 0), np.hypot(4, 0) > 2 + 1)"ここでは表示のみで、実行されません。無料アカウントを作成すると、実行ボタンで自分のラボのターミナルに入力できます。
ひじの曲げ方が2通りあるので解は2つ
目標までの距離 d と、2つのリンクの長さから、三角形ができます。余弦定理で、c = (d の2乗 − l1 の2乗 − l2 の2乗) ÷ (2 × l1 × l2) を求めます。
a2 は、acos(c) か、その符号を逆にした値です。この2つが、ひじを上に曲げる解と、下に曲げる解です。
a2 が決まれば a1 も決まる
a2 を決めたら、a1 は「目標の向き」から「ひじの分のずれ」を引いて求めます。
目標の向きは atan2(y, x) です。ずれは atan2(l2 × sin a2, l1 + l2 × cos a2) です。どちらも、ラジアンで出るので、最後に度へ直します。
例: 目標 (2, 1) の2つの解
| 解 | a1 | a2 | ひじの位置 |
|---|---|---|---|
| ひじを上げる | 0度 | 90度 | (2, 0) |
| ひじを下げる | 約 53.13度 | -90度 | (1.2, 1.6) |
学生が入力するセルは、このプレビューでは空白です。
逆運動学を Python で
import math
# リンクの長さ 2 と 1。目標は (1.5, 1.5) です。
l1 = 2
l2 = 1
x = 1.5
y = 1.5
c = (x * x + y * y - l1 * l1 - l2 * l2) / (2 * l1 * l2)
a2 = math.acos(c)
a1 = math.atan2(y, x) - math.atan2(l2 * math.sin(a2), l1 + l2 * math.cos(a2))
print("a1:", round(math.degrees(a1), 2), "a2:", round(math.degrees(a2), 2))
# 目標を (2.5, 0) に変えて、もう一度実行します。
# a2 はいくつになりますか。
よくある間違い
注意
目標ごとに、角度は1つだけだと考えがちです。実際は、届く範囲の内側なら2つあります。また、c が 1 より大きいと acos は失敗します。先に届くかを調べ、届かないときは None を返します。