こんにちは、shotaです。
アールティが販売するロボットCRANE-X7（クラインエックスセブン）とSciurus17（シューラスセブンティーン）のC++ライブラリに、逆運動学サンプルと重力補償サンプルを追加しました。
この記事ではサンプルの使い方を紹介します。
CRANE-X7とSciurus17のC++ライブラリについて
RTマニピュレータC++ライブラリとサンプル集（rt_manipulators_cpp）は
CRANE-X7とSciurus17に搭載されたサーボモータを直接動かすためのオープンソースソフトウェアです。
このライブラリを使えば、サーボモータのトルクON/OFFや、モータ角度・速度・電流の読み書きなどができます。
また、ロボットのリンクパラメータ（リンク長さや、質量、重心位置等）もライブラリに用意されているため、運動学やトルク制御等の実装が容易です。
「RTマニピュレータC++ライブラリは、CRANE-X7・Sciurus17のROSパッケージと何が違うの？」
と思った方へ簡単な図を用意しました。
ROSパッケージとC++ライブラリの役割
ROSパッケージはアプリケーション開発向けのミドルウェアです。
軌道生成、画像処理等の既存ソフトウェアを組み合わせることで、「カメラで認識した物を運ぶ」、といったアプリケーションを簡単に作れます。
便利な一方で、ロボットのハードウェアを扱う実装は隠されている（ことが多い）ため、制御ソフトの実装には苦労します。
それぞれ特徴があるので、CRANE-X7・Sciurus17のROSパッケージしか使ったことがない人も、ぜひRTマニピュレータC++ライブラリを試してみてください。
低レイヤ側を知ることで、よりROSパッケージやロボットへの理解度が深まると思います。
RTマニピュレータC++ライブラリを試す場合は、下記のブログを参考にセットアップしてください。
解析的に逆運動学を解くサンプル
それでは、今回追加された2つのサンプルを実行します。
まずはじめに、解析的に逆運動学を解くサンプルです。
次のコマンドでライブラリを更新します。
# ライブラリのディレクトリに移動して更新 $ cd rt_manipuators_cpp $ git pull origin main # ライブラリをビルド&インストール $ ./rt_manipulators_lib/build_install_library.bash
新しく追加されたサンプルをビルドし、実行します。
$ cd rt_manipulators_cpp/samples/samples03 $ ./build_samples.bash # 逆運動学サンプルを実行 $ cd bin/ # CRANE-X7の場合 $ ./x7_3dof_inverse_kinematics # Sciurus17の場合 $ ./s17_3dof_inverse_kinematics
実行すると、次のようにCRANE-X7とSciurus17が動作します。
手先の目標位置を変えたい場合は、サンプルのREADMEを参考にプログラムを書き換えてください。
重力補償トルクを入力するサンプル
次のコマンドで重力補償サンプルを実行します。
$ cd rt_manipulators_cpp/samples/samples03/bin # CRANE-X7の場合 $ ./7_gravity_compensation # Sciurus17の場合 $ ./s17_gravity_compensation
実行すると、次のようにCRANE-X7とSciurus17が動作します。
以上、2つのサンプルを紹介しました。
CRANE-X7やSciurus17で運動学・動力学を実践する際に活用してください。
私は「実践ロボット制御」という書籍で運動学・動力学を学び、これらのサンプルを実装しています。
rt_manipulators_dynamics.cppに実装されている関数は、書籍の9章で扱っているロボットの運動方程式から重力補償トルク項を抽出したものです。
重力補償に関連して、10章には「重力補償があるとフィードバックゲインを小さくできる」という記述があります。
これを実践するために、重力補償サンプルに簡単なフィードバック制御（P制御）を追加する例を紹介します。
重力補償サンプルを改造する
3番目と5番目のリンクを動かすサーボモータの角度が45度、-135度となるようにフィードバック制御を追加します。
まず、x7_gravity_compensation.cpp の162行目に次のコードを追記します。
モータの現在角度が目標角度から離れるほど、制御量（電流値）が大きくなる、というフィードバック制御です。
// ここで重力補償分の電流値を計算
samples03_dynamics::gravity_compensation(
links, target_id, torque_to_current, q_list);
// 3番目、5番目のリンクを動かすサーボモータのP制御
const double p_gain = 0.5;
const double target_q3 = M_PI_4; // 目標値45度
const double target_q5 = -(M_PI_2 + M_PI_4); // 目標値-135度
q_list[3] += p_gain * (target_q3 - links[3].q);
q_list[5] += p_gain * (target_q5 - links[5].q);
for (const auto & [target_id, q_value] : q_list) {
hardware.set_current(links[target_id].dxl_id, q_value);
}
ビルドして実行します。
$ cd rt_manipulators_cpp/samples/samples03 $ ./build_samples.bash # 逆運動学サンプルを実行 $ cd bin/ # CRANE-X7の場合 $ ./x7_3dof_inverse_kinematics # Sciurus17の場合 $ ./s17_3dof_inverse_kinematics
実行結果がこちらです。
フィードバック制御が働いていることがわかります。
まとめ
今回の記事では、RTマニピュレータC++ライブラリの解析的に解く逆運動学サンプルと重力補償サンプルを紹介しました。
また、重力補償サンプルの改造例も紹介しました。
今後もサンプルを追加したらブログで紹介します。
