ROS 2 実践コース｜#B2 逆運動学を紙で解いてから、ノードにする

前の週（#B1）では、関節の角度から脚先の位置を求めました。ロボットに歩かせたいときに必要なのは、その逆です。「脚先をここに置きたい」から、関節の角度を求める——これを逆運動学といいます。

この週は、まず紙で解きます。プログラムにする前に紙で解くのは、「解が無い場合」がどう式に現れるかを、自分の手で見るためです。AI に書かせたコードを読むときも、この経験があるかどうかで見え方が変わります。

この記事の位置

この記事は延長コース Bの 2 回分（B3・B4）です。授業で受ける人は、記録を実習レポート（Word）の「回 B3」「回 B4」のシートに分けて書きます。スクリーンショットは、シートの中の「画面：」の欄（実験の表のすぐ下）に貼ります。延長コースのレポートは、コア（第 1〜12 回）とは別の 1 本にします。ファイルは Google Classroom で配られ、コースの最後に Google Classroom で 1 回だけ出します。期限は最後の実習から 1 週間後の 23:59 です。ひとりで学ぶ人も、予想と結果を同じ形でノートに残しておくと、あとで見返せます。

回 B3　紙で解く

脚先の位置から 2 つの関節角を求める式を紙で導き、その式で、届く位置と届かない位置を計算します。次の 5 つをやります。

1. B3-1 式を自分で導きます。下のヒントの余弦定理から cos(θ2) の式を作り、そのあと θ1 の式まで、途中式を書きます。
2. B3-2 3 通りの脚先の位置について、手で計算して表にします。L1・L2 は #B1 で調べた値（もも約 0.060 m、すねは測った値）を使います。
3. B3-3 届かない位置を 2 つ挙げ、そのときの acos の中身がいくつになるかを計算します。
4. B3-4 下の「ライブラリが決めている動く範囲」の表と見比べ、計算では出るのに実物では出せない角度を挙げます。
5. B3-5 回 B4 で使う型の中身を、ros2 interface show で確かめます。

B3-1　式を導く

図の三角形に余弦定理を当てはめて、θ2 の式から作ります。手がかりは次のとおりです。

股関節・膝・脚先の 3 点を結ぶと三角形ができる（図）。
3 辺の長さは、L1（もも）、L2（すね）、r（股関節から脚先までの距離）。

  r^2 = x^2 + z^2                     … 三平方の定理

余弦定理（辺 a・b にはさまれた角を C、向かいの辺を c とする）：

  c^2 = a^2 + b^2 - 2*a*b*cos(C)

ここで、膝の内側の角を φ とすると、θ2（ももから見たすねの角度）は φ とどういう関係にあるか。
θ2 = 0 は、ももとすねがまっすぐ一直線に伸びた姿勢である。

θ2 が出たら、θ1 は「股関節から脚先へ向かう線の角度」から「その線とももの間の角度」を引いて求めます。角度を出すには atan2 を使います（atan2(z, x) は、点 (x, z) へ向かう線の角度です）。

脚先の位置 (x, z) から、関節角 θ1・θ2 を求める（平面 2 リンク）。

  r^2 = x^2 + z^2                                  … 股関節から脚先までの距離の 2 乗
  cos(θ2) = (r^2 - L1^2 - L2^2) / (2 * L1 * L2)
  θ2 = acos( cos(θ2) )
  θ1 = atan2(z, x) - atan2( L2*sin(θ2), L1 + L2*cos(θ2) )

ここで acos に入れる値は -1 以上 1 以下でなければならない。
r が L1 + L2 より大きいと、右辺は 1 を超える。つまり「その位置には届かない」。
r が |L1 - L2| より小さいときも、-1 を下回る（内側に近すぎて折りたためない）。

B3-2　3 通りの位置で計算する

導いた式に、脚先の位置を 3 通り入れて θ1・θ2 を出し、表にします。

位置は自分で選んでかまいません。選ぶときの手がかりは、脚の長さです。股関節から脚先までの距離 r が L1 + L2 より短く、|L1 − L2| より長い位置を選びます。θ2 の acos は 0〜π の値を返すので、膝はいつも同じ向きに折れた解が出てきます（もう 1 つの解は「つまずきやすい点」へ）。

B3-3　届かない位置で計算する

わざと届かない位置を 2 つ選び、acos に入れる値を計算します。

予想：脚先を L1 + L2 より遠くに置こうとすると、acos に入れる値はいくつくらいになるでしょうか。手がかりは、cos の値がとれる範囲（−1 以上 1 以下）と、3 辺のうち 1 辺がほかの 2 辺の和より長いと三角形が作れない、ということです。1 つは遠すぎる位置、もう 1 つは股関節に近すぎる位置を選ぶと、違いが見えます。

B3-4　動く範囲と見比べる

計算で出た角度を、ロボットのライブラリが決めている動く範囲と比べます。

下の表は、ロボットを動かすライブラリ（DOGZILLALib）のコードの中に書かれている制限の値を読み取ったものです。ライブラリから読んだ値で、実機では確かめていません。単位もコードから読んだもので、関節の角度は度、脚先の位置は mm です。

比べるときは、次の 3 つに気をつけます。1 つ目は単位で、計算の角度は rad、表は度です。2 つ目は 0 の姿勢で、ライブラリの 0° と、自分のモデルの θ = 0 が同じ姿勢とは限りません。3 つ目は名前で、表の「下・中・上の関節」が URDF のどの関節に当たるかも、まだ確かめていません。どう対応させたかを、自分の考えとして書いてください。

B3-5　使う型を確かめる

回 B4 のノードが受け取る型と出す型の中身を、ros2 interface show で見ます。

# 回 B4 で使う型の中身を確かめる
ros2 interface show geometry_msgs/msg/Point
ros2 interface show sensor_msgs/msg/JointState

ros2 interface show は、型にどんな項目があり、それぞれが何の型かを表示します。Point は x・y・z の 3 つ（どれも float64）、JointState は name・position・velocity・effort の配列と、時刻の入る header です。どの項目を使い、どれを使わないかを、回 B4 の設計シートに書くために見ておきます。単位は型には書かれていないので、設計シートで決めます。

本質の問い（回 B3）

• 計算で出た角度を、そのままロボットに送ってはいけない理由を、自分の計算で示しなさい。

回 B4　ノードにして、「行って戻る」で確かめる

回 B3 の式を逆運動学のノードにし、#B1 の順運動学のノードとつないで、入れた位置がそのまま戻ってくるかを確かめます。次の 5 つをやります。

1. B4-1 設計シート：入力は脚先の位置、出力は関節角。範囲外のときに何を返すか（送らない／直前の値を保つ／エラーを出す）を決めます。
2. B4-2 AI に実装させて「行って戻る」：逆運動学のノードを作らせます。範囲外の扱いが設計シートのとおりかを、できたコードを読んで確かめます。そのあと #B1 の順運動学ノードとつなぎ、「位置 → 角度 → 位置」を通して、戻ってきた位置と最初に入れた位置の差を 3 通り書きます。
3. B4-3 届かない位置を入れて、設計どおりに止まるかを確かめます。
4. B4-4 ros2 topic hz で、2 つのノードを通したときに途中で落ちていないかを測ります。
5. B4-5 rqt_graph で、2 つのノードのつながりを確かめます。

ロボットには出しません。出し先のトピック名は /joint_command のような試験用の名前にしておき、ロボット側の入口とは別にします。

B4-1　設計シートを書く

AI に渡す前に、ノードの入口と出口、範囲外のときの振る舞いを決めて書きます。下は書き方の例です。「（自分で決める）」「（測った値）」の欄は、自分で埋めます。

ノード名：ik_node（パッケージ名は my_robot。#B1 の fk_node と同じパッケージでよい）
役割：脚先の目標位置から、左前脚の 2 つの関節の角度を計算して出す

入力：/foot_target（geometry_msgs/msg/Point）
  単位：m
  どのフレームで表すか：#B1 の fk_node の出力と同じ（股関節の回転の中心が原点）
  y は使わない（平面のモデルなので）

出力：/joint_command（sensor_msgs/msg/JointState）
  関節の名前：#B1 の fk_node が聞いている 2 つの名前と同じ
  単位：rad
  ロボット側の入口とは別の、試験用の名前にする

脚の長さ（パラメータ）：L1 ＝ 0.060 m、L2 ＝（#B1 の B1-5 で測った値）m

範囲外のとき：
  届かない（acos に入れる値が -1〜1 の外）→ 角度を送らずに、警告を 1 行出す。警告には acos に入れる値も出す
  関節が動ける範囲の外 →（自分で決める）

解が 2 つあるとき：膝をどちら向きに折る解を使うか（自分で決める）

検証の方法：
  1. fk_node とつないで「位置 → 角度 → 位置」を 3 通り通し、差を mm で書く
  2. 届かない位置を入れて、角度が出ないことを確かめる
  3. 5 Hz で位置を流し、/joint_command と /foot_position の Hz を並べる

決め方が分からない欄は、#B1 と同じく、AI に埋めさせずに質問役をしてもらい、自分で答えながら決めます。決めた理由は、レポートの「使った AI とプロンプト」の欄に書きます。

B4-2　「行って戻る」を通す

逆運動学のノードと順運動学のノードをつなぎ、位置を入れて、同じ位置が戻ってくるかを見ます。

# 端末 1：逆運動学のノード（位置 → 角度）
ros2 run my_robot ik_node

# 端末 2：#B1 の順運動学のノード（角度 → 位置）。/joint_states のかわりに /joint_command を聞かせる
ros2 run my_robot fk_node --ros-args -r joint_states:=joint_command

# 端末 3：戻ってきた位置を見る
ros2 topic echo /foot_position

# 端末 4：入れたい位置を 1 回だけ入れる（x・z の値を変えて 3 通り）
ros2 topic pub -1 /foot_target geometry_msgs/msg/Point "{x: 0.15, y: 0.0, z: -0.05}"

• 端末 2 の -r joint_states:=joint_command：#B1 の fk_node は /joint_states を聞くように作ってあります。コードは変えずに、聞く先を ik_node の出力 /joint_command に付け替えます（リマップ）
• 端末 4 の -1：位置を 1 回だけ送ります。x・z の値を変えて、B3-2 で計算した 3 通りを入れます
• y: 0.0：設計シートで y は使わないと決めたので、0 を入れておきます

予想：戻ってきた位置と入れた位置の差は、何 mm くらいになるでしょうか。手がかりは、2 つのノードが同じ式の逆向きと順向きで、同じ L1・L2 を使っていることです。約束（角度の向き・0 点・単位）がそろっていれば、差はどうなるはずかを考えます。

結果を見る（予想を書き、自分で実行してから開く）

この画面の計算は、L1 と L2 をどちらも 0.1 m にしています。実物のももは約 0.06 m なので、皆さんの角度の値は画面とは変わります。

同じ計算を往復させると、式の間違いは差として出てきます。差がほとんど 0 なら、2 つのノードは同じ約束（角度の向き・0 点・単位）で書かれているということです。差が大きいときは、まず符号と単位を疑います。

B4-3　届かない位置を入れる

B3-3 で計算した届かない位置を入れて、ノードが設計シートのとおりに振る舞うかを見ます。

# 端末 4：届かない位置を 1 回だけ入れる
ros2 topic pub -1 /foot_target geometry_msgs/msg/Point "{x: 0.40, y: 0.0, z: -0.02}"

# 端末 3 の /foot_position と、端末 1 の ik_node の表示を見る
# 角度が出ていないことは、別の端末でも確かめられる
ros2 topic echo /joint_command

予想：端末 1 の ik_node と、端末 3 の /foot_position には、それぞれ何が出るでしょうか。手がかりは、自分の設計シートの「範囲外のとき」の行と、B3-3 で手で計算した acos の中身の値です。

結果を見る（予想を書き、自分で実行してから開く）

画面の 7.0200 は、L1 = L2 = 0.1 m として (0.40, −0.02) を入れたときの値です。自分の L1・L2 で手計算した値と、ノードが出した値が合っているかを見ます。

B4-4　Hz を並べて測る

位置を流し続け、2 つのノードを通ったあとの Hz が、送った Hz と同じかを測ります。

# 端末 4：同じ位置を 1 秒に 5 回（5 Hz）流し続ける（止めるときは Ctrl+C）
ros2 topic pub -r 5 /foot_target geometry_msgs/msg/Point "{x: 0.15, y: 0.0, z: -0.05}"

# 端末 5：ik_node が出した角度の Hz
ros2 topic hz /joint_command

# 端末 6：fk_node が出した、戻ってきた位置の Hz
ros2 topic hz /foot_position

-r 5 は、同じメッセージを 1 秒に 5 回送り続ける指定です（--rate 5 と書いても同じです）。#A2 でやったように、送り側と処理側の Hz を並べて見ます。端末 1〜3 は B4-2 のまま動かしておきます。

予想：/joint_command と /foot_position は、それぞれ何 Hz になるでしょうか。手がかりは、2 つのノードがどちらも「1 つ受け取ったら 1 つ出す」作りになっていることです。

結果を見る（予想を書き、自分で実行してから開く）

B4-5　つながりを見る

rqt_graph を開き、2 つのノードのつながりを確かめます。

rqt_graph

/foot_target → ik_node → /joint_command → fk_node → /foot_position と、1 本の道になっているかを見ます。fk_node に /joint_states から矢印が来ていたら、リマップが効いていません。

この回で撮っておく画面

動画は撮りません。そのかわり、下の画面を撮って残します。授業で受ける人は、実習レポートの「回 B3」の、それぞれの「画面：」の欄に貼ります。ひとりで学ぶ人も、同じ画面を残しておくと、あとで値を見直せます。最初の 1 枚は、実習を始める前に端末で TZ=Asia/Tokyo date と打ち、日付と時刻が出た画面です。授業の日に実習したことの記録になります。講義用の環境の時計は世界標準時（UTC）なので、TZ=Asia/Tokyo を付けて日本時間で出します。

• TZ=Asia/Tokyo date の画面（回の頭）
• 導いた式（写真）（実験 B3-1 の欄）
• 3 通りの計算表（写真）（実験 B3-2 の欄）
• 届かない位置の計算（写真）（実験 B3-3 の欄）
• 動く範囲の表と見比べたところ（写真）（実験 B3-4 の欄）
• ros2 interface show で使う型を確かめた画面（実験 B3-5 の欄）

この回で撮っておく画面

動画は撮りません。そのかわり、下の画面を撮って残します。授業で受ける人は、実習レポートの「回 B4」の、それぞれの「画面：」の欄に貼ります。ひとりで学ぶ人も、同じ画面を残しておくと、あとで値を見直せます。最初の 1 枚は、実習を始める前に端末で TZ=Asia/Tokyo date と打ち、日付と時刻が出た画面です。授業の日に実習したことの記録になります。講義用の環境の時計は世界標準時（UTC）なので、TZ=Asia/Tokyo を付けて日本時間で出します。

• TZ=Asia/Tokyo date の画面（回の頭）
• 設計シート（写真）（実験 B4-1 の欄）
• 「行って戻る」の差が出ている画面（実験 B4-2 の欄）
• 届かない位置を入れたときの表示（実験 B4-3 の欄）
• ros2 topic hz（実験 B4-4 の欄）
• rqt_graph（実験 B4-5 の欄）

つまずきやすい点

まとめ

次の #B3 では、IMU で胴体の傾きを測り、水平に保つノードを作ります。机の上で確かめてから、先生の立ち会いのもとで床に降ろす、という順で進めます。