延長コース B は、コアの第 4 週でやった「座標と時間」を、脚 1 本の計算にまで広げるコースです。ロボットの形は URDF という書式で書かれていて、TF はそこから「どのフレームから見た位置か」を計算しています。この週は、その中身を読み、自分で計算した脚先の位置と、TF が返す位置が合うかを確かめます。
この記事の位置
| 項目 | 内容 |
|---|---|
| コース | 延長コース B「脚の運動学」（3 週・回 B1〜B6） |
| この記事 | 第 1 週（回 B1・B2）。このあと #B2（逆運動学）、#B3（IMU と水平保持）と続きます |
| 前に済ませておくこと | コアの第 1〜6 週。とくに #09（QoS・TF・タイムスタンプ） |
| ロボット | この記事の範囲では、立っているロボットから値を読むだけです。脚の指令は出しません |
| この週で身につけること | URDF と TF を読み、脚先の位置を自分で計算して、TF の値と突き合わせられること |
ロボットを使うときの約束：このコースでも、ロボットを動かすのは先生が「始めてよい」と言ったときだけです。ひとりで学ぶ場合は、ほかの人に見ていてもらい、すぐ電源を切れる状態にしてから進めてください。
この記事は延長コース Bの 2 回分（B1・B2）です。授業で受ける人は、記録を実習レポート（Word）の「回 B1」「回 B2」のシートに分けて書きます。スクリーンショットは、シートの中の「画面：」の欄（実験の表のすぐ下）に貼ります。延長コースのレポートは、コア（第 1〜12 回）とは別の 1 本にします。ファイルは Google Classroom で配られ、コースの最後に Google Classroom で 1 回だけ出します。期限は最後の実習から 1 週間後の 23:59 です。ひとりで学ぶ人も、予想と結果を同じ形でノートに残しておくと、あとで見返せます。
回 B1 URDF と TF を読む
ロボットの形の定義（URDF）と TF の木を読み、脚の部品のつながりと寸法を調べます。B1-1〜B1-6 の 6 つをやります。ロボットは立たせたまま（または置いたまま）で、値を読むだけです。
この回の「結果を見る」に載せた画面は、ロボットの代わりに手元で形の定義を配って確かめたものです。読む側から見れば、ロボットが出しているのか手元が出しているのかは区別がつきません。皆さんはロボットにつないだ状態で、同じコマンドを打ちます。
B1-1 リンクと関節の数を数える
URDF を受け取ってファイルに保存し、リンク（部品）と関節（つなぎ目）がいくつあるかを数えます。
# URDF（ロボットの形の定義）を 1 回だけ受け取って保存する
# --field data --full-length の 2 つが要る（理由は「つまずきやすい点」へ）
ros2 topic echo /robot_description --once --field data --full-length > ~/urdf.txt
wc -l ~/urdf.txt
grep -c "<link" ~/urdf.txt
grep -c "<joint" ~/urdf.txt
# 関節の名前だけを並べる（メッシュのファイル名にも name= が付くので、絞り込む）
grep "<joint" ~/urdf.txt | grep -o 'name="[a-z_]*"'
ros2 topic echo /robot_description --once：ロボットが配っている形の定義を 1 回だけ受け取ります。> ~/urdf.txtで、画面に出すかわりにファイルへ書き込みますwc -l：ファイルの行数を数えます。数行しかなければ、うまく保存できていませんgrep -c "<link"：<linkを含む行の数を数えます。URDF ではリンク 1 つにつき 1 行なので、これがリンクの数になります。<jointも同じです- 最後の行：
<jointの行からname="…"の部分だけを抜き出します。B1-3 で使います
予想：脚 1 本に関節はいくつあるでしょうか。手がかりは、実物の脚の動き方です。脚を前後に振る、横に開く、膝を曲げる——ロボットの脚を見て、別々に回る軸がいくつあるかを数えてから、コマンドの結果と比べます。関節の数が「脚の数 × 1 本あたりの関節の数」より多いときは、残りが何と何をつないでいるのかも考えます。
結果を見る（予想を書き、自分で実行してから開く）
<link が 17、<joint が 16。脚 1 本に関節が 3 つで 4 本ぶん 12、残りの 4 つは base_footprint・カメラ・LiDAR・IMU と胴体をつなぐ固定の関節B1-2 胴体から脚先までの道をたどる
TF の木を PDF の図にして、胴体（base_link）から脚先までのフレーム名を、順に書き出します。
# 作業用のフォルダへ移動してから、TF の木を PDF にする（実行した場所に frames_*.pdf ができる）
mkdir -p ~/leg_check && cd ~/leg_check
ros2 run tf2_tools view_frames
ls
# できた PDF を開く
atril frames_*.pdf
view_frames は 5 秒ぶん TF を聞いてから図を作り、コマンドを打ったフォルダにファイルを置きます。そのため、先に専用のフォルダを作って移動しています。ls でファイルができたことを確かめ、atril（PDF を見るアプリ）で開きます。第 4 週（#09）で base_link → laser_frame をたどったのと同じ読み方です。
view_frames は 5 秒ぶん TF を聞いてから図を作る。できるのは .gv と .pdf の 2 つで、実行したディレクトリに置かれる（この画面は作業用のフォルダを作らずに実行している）予想：脚 1 本は、胴体から何段のフレームでつながっているでしょうか。手がかりは B1-1 で数えた「脚 1 本あたりの関節の数」です。関節 1 つが、フレームとフレームを 1 段つなぎます。
結果を見る（予想を書き、自分で実行してから開く）
base_footprint → base_link から4 本の脚が枝分かれし、それぞれ hip → upper_leg → lower_leg と 3 段に伸びている。カメラ・IMU・LiDAR は胴体から直接ぶら下がるB1-3 関節名とリンク名を見比べる
B1-1 の最後の行で出した関節名の一覧と、B1-2 の木の図のリンク名を、左右・前後の 4 本で見比べます。そろっていないところが 1 つあります。見つけて、どこがどう違うかを書いてください。
手がかり：名前は「どの脚か」「脚のどこか」「関節かリンクか」の 3 つの部分でできています。1 つの部分ごとに、12 本を縦に並べて見比べると見つけやすくなります。
名前が規則的でないと、プログラムで「左前脚のすね」を機械的に組み立てられません。これは、あとで自分のノードを書くときに効いてきます。左前脚のももとすねの関節名は、回 B2 で使うので控えておきます。
結果を見る（予想を書き、自分で実行してから開く）
..._joint で終わっているのに、1 つだけ ..._link で終わっている。これは関節の名前なので、_link は書き間違いであるB1-4 胴体からすねまでの距離を読む
tf2_echo で、胴体から 4 本それぞれのすねのリンクまでの位置を読み、表にします。
# 胴体から、左前脚のすねまでの位置と向きを読む（止めるときは Ctrl+C）
ros2 run tf2_ros tf2_echo base_link lf_lower_leg_link
# 残りの 3 本も、lf を rf・lh・rh に変えて同じように読む
# （脚の名前は B1-2 の木の図で確かめる）
tf2_echo A B は「A から見た B の位置と向き」を 1 秒ごとに出し続けます。出てきた Translation（位置。x・y・z の順で、単位は m）の 3 つの数を、脚ごとに書き写します。Rotation は向きで、ここでは使いません。
予想：4 本の値は、左右で y の符号だけが違うでしょうか。前後ではどうでしょうか。手がかりは、base_link の向きです（#09 で見たとおり、x が前、y が左、z が上）。胴体の真ん中から見て、左前脚と右前脚はどちらの向きに離れているかを考えます。
結果を見る（予想を書き、自分で実行してから開く）
B1-5 すねの長さを実物で測る
ももとすねの長さを巻尺で測り、URDF の値と比べます。ロボットは動かしません。どこからどこまでを測ったのか（関節の中心から足先まで、など）が、写真に残るように測ってください。
ももは、URDF では約 60 mm です（すねの関節の <origin> の z の値。grep -A3 lower_leg ~/urdf.txt などで、自分の urdf.txt の中の値を確かめてください）。測った値と合うかを比べ、合わないときは、測った場所が違うのかを考えます。
すねは、URDF に足先のリンクが無いので、長さの値が入っていません。そのため測った値を、このあとの計算（回 B2 の L2）にそのまま使います。どこを足先とみなしたか（丸みの先端か、丸みの中心か）も書いておきます。
B1-6 壊す実験：つながっていないフレームを指定する
tf2_echo に、直接はつながっていない 2 つのフレームと、存在しないフレームを渡して、何が起きるかを見ます。
ros2 run tf2_ros tf2_echo lf_lower_leg_link rf_lower_leg_link
ros2 run tf2_ros tf2_echo base_link nonexistent_link
1 行目は、左前脚のすねから右前脚のすねを見ます。2 つは直接はつながっていません。2 行目の nonexistent_link は、木のどこにも無い名前です。どちらも、止めるときは Ctrl+C です。
予想：それぞれ、値が出るでしょうか、エラーで止まるでしょうか、待ち続けるでしょうか。手がかりは B1-2 の木の図です。2 つのフレームの間に、木の枝をたどって行ける道があるかを見てください。出たメッセージはそのまま書き写します。
結果を見る（予想を書き、自分で実行してから開く）
frame does not exist と言いながら待ち続けるTF は「木」なので、間の道がつながっていれば計算できます。どちらの例が計算でき、どちらができなかったかを、木の図と見比べて説明してください。
この回で撮っておく画面
動画は撮りません。そのかわり、下の画面を撮って残します。授業で受ける人は、実習レポートの「回 B1」の、それぞれの「画面：」の欄に貼ります。ひとりで学ぶ人も、同じ画面を残しておくと、あとで値を見直せます。最初の 1 枚は、実習を始める前に端末で TZ=Asia/Tokyo date と打ち、日付と時刻が出た画面です。授業の日に実習したことの記録になります。講義用の環境の時計は世界標準時（UTC）なので、TZ=Asia/Tokyo を付けて日本時間で出します。
TZ=Asia/Tokyo dateとrobot-status（回の頭）- リンクと関節を数えた画面（実験 B1-1 の欄）
- TF の木（実験 B1-2 の欄）
- 関節名の一覧（実験 B1-3 の欄）
tf2_echoの出力（実験 B1-4 の欄）- 巻尺で測っているところ（写真）（実験 B1-5 の欄）
- つながっていないフレームを指定したときの表示（実験 B1-6 の欄）
本質の問い（回 B1）
- 同じ 1 点の位置を、2 つのフレームで表したときに数値がどう変わるか。自分の
tf2_echoの値で説明しなさい。
考え方のすじ道（答えを書いてから開く）
位置の数値は、「どこを原点にして、どちらを向きの基準にするか」で変わります。物そのものが動いたわけではありません。tf2_echo A B は「A から見た B の位置と向き」を返すので、A を変えれば数値は変わります。
レポートでは、この一般論ではなく、自分が読んだ 2 組の値（たとえば base_link から見たすねと、もものリンクから見たすね）を並べて、どこがどれだけ違うかを書いてください。
回 B2 順運動学：角度から脚先の位置を出す
関節の角度を受け取って脚先の位置を出すノードを作り、紙の計算と TF の値で確かめます。流れは、コアの #04 と同じ「設計シート → AI → 検証」で、次の 5 つに分けて進めます。
- B2-1 設計シート：入力（
/joint_statesの角度）、出力（脚先の位置と、それをどのフレームで表すか）、単位（rad か度か）、範囲を決めます。出力の型は、コース A で作った自作メッセージでもgeometry_msgs/msg/PointStampedでもかまいません。 - B2-2 AI に実装させて、角度を手で入れる：2 つのリンクの平面モデルで脚先を求めるノードを作らせ、角度を手で 3 通り入れて、出てくる位置を見ます。
- B2-3 紙で検算する：三角関数の中身は、自分で 1 通りだけ手で計算して、ノードの出力と比べます。
- B2-4 TF と比べる：ロボットの本物の角度で膝の位置を計算し、TF で見た膝の位置との差を mm で書きます。差の原因を、ももの長さ・関節の 0 点のずれ・角度の向き・平面で近似したこと、のどれで説明できるかを考えます。
- B2-5 つながりを見る：
rqt_graphで、自分のノードがどのトピックを聞いているかを確かめます。
ロボットは動かしません。立っている（または置いてある）ままの姿勢で値を読み、計算と比べます。
平面 2 リンクのモデル
計算に使う式を先に確かめます。脚を 2 本の棒とみなし、横から見た平面の中だけで考えます。
脚を、長さ L1（もも）と L2（すね）の 2 本の棒として考える。
股関節を原点、θ1 をももの角度、θ2 をすねの角度（ももから見た相対角）とすると、
x = L1*cos(θ1) + L2*cos(θ1 + θ2)
z = L1*sin(θ1) + L2*sin(θ1 + θ2)
右辺の 1 つ目の項（L1 の項）が膝の位置、2 つの項を足したものが脚先の位置になる。
θ の向き（どちらを正とするか）と、0 のときにどの姿勢になるかは、
URDF の <axis> と <origin> を見て自分で決めて、設計シートに書く。
実物の脚は 3 次元に動くので、これは近似です。股関節を横に開く関節（脚 1 本の 3 つの関節のうちの 1 つ）は、このモデルでは 0 のままとみなしています。どこを近似したかを書いておくと、差が出たときに原因を絞れます。
B2-1 設計シートを書く
AI に渡す前に、ノードの入口と出口を決めて書きます。下は書き方の例です。「（自分で決める）」「（B1-5 で測った値）」の欄は、自分で埋めます。
ノード名：fk_node（パッケージ名は my_robot。自分で付けた名前でよい）
役割：左前脚の 2 つの関節の角度から、脚先の位置を計算して出す
入力：/joint_states（sensor_msgs/msg/JointState）
使う関節：もも（B1-3 で控えた名前）、すね（B1-3 で控えた名前）
単位：rad
手で角度を入れて試すときは、起動時に -r joint_states:=/joint_states_test でつなぎ替える
出力：/foot_position（geometry_msgs/msg/PointStamped）
単位：m
どのフレームで表すか：股関節（ももの関節）の回転の中心を原点にする。向きは（自分で決める）
脚の長さ（パラメータにして、起動時に変えられるようにする）：
L1（もも）＝ 0.060 m（URDF から読んだ値）
L2（すね）＝（B1-5 で測った値）m
角度の向きと 0 の姿勢：（URDF の <axis> と <origin> を見て自分で決める）
使う関節の名前が届いたメッセージに無いとき：位置を出さずに、警告を 1 行出す
検証の方法：
1. 角度を 3 通り手で入れ、出た位置を紙の計算と比べる
2. L2 を 0 にして起動し、膝の位置を TF と比べる
| 欄 | 書くこと | なぜ書くか |
|---|---|---|
| ノード名・役割 | 何という名前で、何をするノードか | あとで rqt_graph や ros2 node list で見たとき、どれが自分のノードかを見分けるため |
| 入力 | トピック名・型・使う関節の名前・単位 | /joint_states には 12 本ぶんの角度が入っている。どの 2 本を使うかを名前で書かないと、AI は順番（何番目か）で取り出すコードを書くことがある |
| 手で試すときのつなぎ替え | -r joint_states:=/joint_states_test | ロボットの本物の /joint_states と、手で入れた角度が混ざらないようにするため（B2-2） |
| 出力 | トピック名・型・単位・どのフレームで表すか | 位置の数は、原点と向きを決めないと意味が決まらない（回 B1 の本質の問い）。TF と比べるときにも要る |
| 脚の長さ | L1・L2 の値と、どこから取った値か | すねの長さは URDF に無いので、測った値だと書いておく。パラメータにしておけば、測り直したときにコードを直さずに済む。B2-4 では L2 を 0 にして膝の位置を出す |
| 角度の向きと 0 の姿勢 | どちら回りを正にするか、0 のときに脚がどちらを向いているか | ここが TF と食い違うと、計算は正しくても値が合わない。差が出たときにまず疑うところ |
| 名前が無いとき | 使う関節の名前がメッセージに無いときにどうするか | 書いておかないと、AI は落ちるコードか、黙って 0 を使うコードのどちらかを勝手に選ぶ |
| 検証の方法 | 何と何を比べて確かめるか | 作る前に確かめ方を決めておくと、できたものを見てから都合のよい確かめ方を選ぶことを防げる |
決め方が分からない欄は、AI に埋めさせず、質問役をしてもらいます。たとえば次のように頼み、出てきた質問に自分で答えながら決めます。決めた理由は、レポートの「使った AI とプロンプト」の欄に書きます。
次の設計シートで、まだ決めていない欄があります。
答えを書かずに、私が決められるように質問を 1 つずつしてください。
私が答えたら、その答えで矛盾が出ないかだけを指摘してください。
（ここに設計シートを貼る）
B2-2 AI に実装させて、角度を手で入れる
設計シートを AI に渡してノードを作らせ、角度を手で入れて、出てくる位置を見ます。ビルドの手順はコアの #04 と同じです。
角度は /joint_states ではなく、試験用の /joint_states_test に入れます。ロボットにつないでいると、ロボットの本物の /joint_states が、ブリッジを通って 1 秒に 5 回（5 Hz）PC に届いています（ロボット側のブリッジの設定 robot.json5 で、/joint_states は 5 Hz までに間引いてあります）。同じ名前に手で 1 回だけ入れても、0.2 秒もしないうちに本物の値で上書きされ、自分のノードの出力に 2 種類の値が混ざってしまいます。そこで、手で入れる角度は別の名前に出し、自分のノードのほうをつなぎ替えて、その名前を聞かせます。
# 端末 1：自分のノードを、/joint_states ではなく /joint_states_test を聞くようにつなぎ替えて起動する
# （my_robot・fk_node は、自分で付けたパッケージ名・ノード名に置き換える）
ros2 run my_robot fk_node --ros-args -r joint_states:=/joint_states_test
# 端末 2：計算の結果を見る
ros2 topic echo /foot_position
# 端末 3：角度を 1 回だけ入れる
# 関節名は B1-3 で控えたものに置き換える
ros2 topic pub -1 /joint_states_test sensor_msgs/msg/JointState \
"{name: ['<ももの関節名>', '<すねの関節名>'], position: [-0.5236, 1.0472]}"
--ros-args -r joint_states:=/joint_states_test：ノードのコードは変えずに、「joint_statesを聞く」ところを「/joint_states_testを聞く」に付け替えます（リマップ）ros2 topic echo /foot_position：ノードが出した脚先の位置を見ます。角度を入れる前に開いておきますros2 topic pub -1：メッセージを 1 回だけ送って終わりますnameとposition：同じ順番で対応します。1 つ目の名前の角度が 1 つ目の数です。単位は rad です
手で入れた角度は、ロボットの脚には届きません。理由はブリッジの設定にあります。PC 側のブリッジの設定（pc.json5）は、PC の中で /joint_states を送るノードがいても、それをロボットへ送り出さないように止めています。さらにロボット側のブリッジは、PC から受け取る名前を /cmd_vel と /sakigake/ で始まるものだけに限っています（robot.json5 の許可リスト）。/joint_states_test もこの許可リストに無いので、ロボットには渡りません。ros2 topic info -v で受け手の数を見ても PC の中のことしか分からないので、届かないことの確かめにはなりません。
入れる角度は、たとえば次の 3 通りです（単位は rad。-0.5236 は −30°、1.0472 は 60°、1.5708 は 90°）。position: [0.0, 0.0]、[-1.5708, 1.5708]、[-0.5236, 1.0472]。
予想：打つ前に、それぞれで /foot_position の x・z がいくつになるかを書いておきます。手がかりは上の式です。θ1・θ2 がどちらも 0 なら、2 本の棒はまっすぐ一直線に並ぶので、脚先は原点から L1 + L2 の距離にあります。残りの 2 通りは、B2-3 の紙の計算で 1 通りを確かめます。
結果を見る（予想を書き、自分で実行してから開く）
/joint_states にそのまま入れ、関節名も hip・knee と短くしている。ロボットにつないだ授業では、本文のとおり /joint_states_test に入れる画面の値は L1 と L2 を 0.1 m にしたときのものです。実物のももは約 0.06 m なので、皆さんの値は変わります。
B2-3 紙で検算する
B2-2 で入れた角度から 1 通りを選び、x と z を手で計算して、ノードの出力と比べます。
cos・sin の値は電卓で出してかまいませんが、θ1 + θ2 を足すところ、どちらの項が膝でどちらが脚先か、単位を rad のまま入れているかは自分で書いて確かめます。ノードの出力と mm 単位で合っていれば、AI が書いた三角関数の部分は式どおりです。
B2-4 TF と比べる
今度は手で入れた角度ではなく、ロボットの本物の角度で計算し、TF が出す位置と比べます。
比べるのは脚先ではなく膝の位置です。TF の木には、脚先のフレームがありません（B1-5 で見たとおり、URDF に足先のリンクが無いため）。すねのリンク lf_lower_leg_link の原点は膝にあるので、TF で読めるのは膝の位置までです。計算のほうも、式の L1 の項（膝の位置）だけを出すため、L2 を 0 にして起動します。
# 端末 1：今度はつなぎ替えずに起動し、ロボットの本物の /joint_states を聞く
# L2 を 0 にすると、計算の L1 の項だけ＝膝の位置が出てくる
ros2 run my_robot fk_node --ros-args -p L2:=0.0
# 端末 2：計算した膝の位置
ros2 topic echo /foot_position
# 端末 3：TF で見た膝の位置（すねのリンクの原点は膝にある）
# lf_hip_link は、もものリンクの 1 つ上のフレーム。名前は B1-2 の木の図で確かめる
ros2 run tf2_ros tf2_echo lf_hip_link lf_lower_leg_link
-p L2:=0.0：パラメータ L2 を 0 にして起動します。設計シートで L1・L2 をパラメータにしたのはこのためです- つなぎ替え（
-r）を付けないので、ノードはロボットから届く本物の/joint_statesを聞きます tf2_echo lf_hip_link lf_lower_leg_link：ももの付け根側のフレームから見た、膝の位置です。この値には、もものリンクの付け根までのずれ（URDF のlf_upper_leg_jointの<origin>）が足されているので、その分を引いてから比べます
予想：差は何 mm くらいになるでしょうか。手がかりは、どちらも同じ URDF の値（もも約 60 mm）と同じ角度から計算していることです。同じ材料で計算しているなら、差が出るとすれば、角度の向きや 0 の姿勢の決め方、平面で近似したところのどれかです。
差を mm で書き、その原因を書きます。なお、すねの長さと足先の丸みは膝の位置には効きません。脚先の計算にどれくらい効くかは、B1-5 で測った値を使って別に見積もります。
B2-5 つながりを見る
rqt_graph を開き、自分のノードがどのトピックを聞き、どのトピックに出しているかを確かめます。
rqt_graph
B2-2 のつなぎ替えをしたときは /joint_states_test から、B2-4 では /joint_states から矢印が来ているはずです。どちらの状態で撮ったかを、画面に添えて書きます。
この回で撮っておく画面
動画は撮りません。そのかわり、下の画面を撮って残します。授業で受ける人は、実習レポートの「回 B2」の、それぞれの「画面：」の欄に貼ります。ひとりで学ぶ人も、同じ画面を残しておくと、あとで値を見直せます。最初の 1 枚は、実習を始める前に端末で TZ=Asia/Tokyo date と打ち、日付と時刻が出た画面です。授業の日に実習したことの記録になります。講義用の環境の時計は世界標準時（UTC）なので、TZ=Asia/Tokyo を付けて日本時間で出します。
TZ=Asia/Tokyo dateとrobot-status（回の頭）- 設計シート（写真）（実験 B2-1 の欄）
- 手打ちした
ros2 topic pubと計算結果を並べた画面（実験 B2-2 の欄） - 紙の計算（写真）（実験 B2-3 の欄）
- TF の値と計算値の差（実験 B2-4 の欄）
- rqt_graph（実験 B2-5 の欄）
つまずきやすい点
| 症状 | 見るところ |
|---|---|
/robot_description が出てこない | ロボットにつながっているか（ros2 topic list）。第 3 週の接続手順に戻る。このトピックは 1 回しか流れないことがあるので、--once で待つ |
urdf.txt が数行しかない／grep -c が 0 になる | ros2 topic echo は長い文字列を途中で切る。--full-length を付ける。さらに --field data が無いと、改行が \n という文字のまま 1 行に詰まり、wc -l でも grep でも数えられない |
view_frames の PDF が見つからない | 実行したディレクトリにできる。ls -lt で新しいファイルを探す |
tf2_echo が値を返さない | フレーム名のつづり。木の図と 1 文字ずつ見比べる。tf2_echo に渡すのはリンク（フレーム）の名前で、関節の名前ではない。B1-3 の一覧（..._joint）から写していないか |
| 計算値と TF の差が大きい | 角度の向き（符号）と 0 点。URDF の <axis> を見て、設計シートに書いた向きと合っているか |
| 手で入れた角度の結果が、すぐ別の値に変わる | /joint_states に直接入れていないか。ロボットの本物の値が 5 Hz で上書きする。/joint_states_test に入れ、ノードを -r joint_states:=/joint_states_test で起動する |
| 単位が合わない | /joint_states は rad。度で計算していないか |
ros2 topic pub の YAML でエラー | 関節名の配列と position の配列の長さがそろっているか。名前は B1-3 で控えたもの |
まとめ
| やったこと | わかったこと |
|---|---|
| URDF を読む | ロボットの形は、リンクと関節の並びとして書かれている |
| TF の木をたどる | 胴体から脚先まで、フレームの鎖でつながっている。間が切れていれば計算できない |
| 実物を測る | URDF の値は、実物と一致しているとは限らない（すねの長さのように、そもそも入っていないこともある） |
| 順運動学を作る | 角度が決まれば脚先の位置は決まる。近似したところは自分で書いておく |
| 手で入れる角度は別の名前に出す | 本物のデータが流れている名前に手で入れると、混ざって確かめにならない。リマップで付け替える |
| TF と比べる | 自分の計算を、動いているシステムの値で確かめられる。ただし比べられるのは TF にあるフレーム（ここでは膝）まで |
次の #B2 では、逆に「脚先の位置から関節の角度を求める」計算を、まず紙で解きます。そこで、計算では出るのに実物では出せない角度があることが見えてきます。